(in branch) Merged from trunk r7960-r8057
Richard Roberts
parent
fd5b040385
commit
3b139cbae2
67
.cproject
67
.cproject
|
|
@ -36,6 +36,9 @@
|
|||
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.1629258328" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base">
|
||||
<option id="gnu.cpp.compiler.option.include.paths.1552452888" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
|
||||
<listOptionValue builtIn="false" value=""${workspace_loc:/gtsam}""/>
|
||||
<listOptionValue builtIn="false" value="/usr/include/c++/4.6.1"/>
|
||||
<listOptionValue builtIn="false" value="/usr/include/c++/4.6"/>
|
||||
<listOptionValue builtIn="false" value="/usr/include/c++/4.6/backward"/>
|
||||
</option>
|
||||
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
|
||||
</tool>
|
||||
|
|
@ -681,6 +684,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check j5" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -2029,6 +2048,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -2045,6 +2080,38 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testSpirit.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testWrap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cproject>
|
||||
|
|
|
|||
109
configure.ac
109
configure.ac
|
|
@ -30,8 +30,11 @@ DX_PS_FEATURE(OFF)
|
|||
DX_INIT_DOXYGEN(gtsam)
|
||||
|
||||
# Check for OS
|
||||
AC_CANONICAL_HOST # needs to be called at some point earlier
|
||||
# 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],
|
||||
|
|
@ -44,20 +47,6 @@ AC_ARG_ENABLE([debug],
|
|||
|
||||
AM_CONDITIONAL([DEBUG], [test x$debug = xtrue])
|
||||
|
||||
|
||||
AC_CANONICAL_HOST
|
||||
# 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],
|
||||
|
|
@ -91,6 +80,77 @@ AC_ARG_ENABLE([install_cppunitlite],
|
|||
|
||||
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
|
||||
|
|
@ -115,25 +175,6 @@ 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],
|
||||
[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])
|
||||
|
||||
# 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])
|
||||
|
||||
AC_CONFIG_FILES([CppUnitLite/Makefile \
|
||||
wrap/Makefile \
|
||||
gtsam/3rdparty/Makefile \
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
800 600 -1119.61507797 1119.61507797 399.5 299.5
|
||||
800 600 1119.61507797 1119.61507797 399.5 299.5
|
||||
|
|
|
|||
|
|
@ -1,12 +0,0 @@
|
|||
10
|
||||
ttpy10.feat
|
||||
ttpy20.feat
|
||||
ttpy30.feat
|
||||
ttpy40.feat
|
||||
ttpy50.feat
|
||||
ttpy60.feat
|
||||
ttpy70.feat
|
||||
ttpy80.feat
|
||||
ttpy90.feat
|
||||
ttpy100.feat
|
||||
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
10
|
||||
ttpy10.pose
|
||||
ttpy20.pose
|
||||
ttpy30.pose
|
||||
ttpy40.pose
|
||||
ttpy50.pose
|
||||
ttpy60.pose
|
||||
ttpy70.pose
|
||||
ttpy80.pose
|
||||
ttpy90.pose
|
||||
ttpy100.pose
|
||||
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
0.939693 0.34202 0 0
|
||||
-0.241845 0.664463 -0.707107 0
|
||||
-0.241845 0.664463 0.707107 0
|
||||
34.202 -93.9693 100 1
|
||||
0.93969 0.24185 -0.24185 34.202
|
||||
0.34202 -0.66446 0.66446 -93.969
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
-0.939693 -0.34202 0 0
|
||||
0.241845 -0.664463 -0.707107 0
|
||||
0.241845 -0.664463 0.707107 0
|
||||
-34.202 93.9693 100 1
|
||||
-0.93969 -0.24185 0.24185 -34.202
|
||||
-0.34202 0.66446 -0.66446 93.969
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
0.766044 0.642788 0 0
|
||||
-0.454519 0.541675 -0.707107 0
|
||||
-0.454519 0.541675 0.707107 0
|
||||
64.2788 -76.6044 100 1
|
||||
0.76604 0.45452 -0.45452 64.279
|
||||
0.64279 -0.54168 0.54168 -76.604
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
0.5 0.866025 0 0
|
||||
-0.612372 0.353553 -0.707107 0
|
||||
-0.612372 0.353553 0.707107 0
|
||||
86.6025 -50 100 1
|
||||
0.5 0.61237 -0.61237 86.603
|
||||
0.86603 -0.35355 0.35355 -50
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
0.173648 0.984808 0 0
|
||||
-0.696364 0.122788 -0.707107 0
|
||||
-0.696364 0.122788 0.707107 0
|
||||
98.4808 -17.3648 100 1
|
||||
0.17365 0.69636 -0.69636 98.481
|
||||
0.98481 -0.12279 0.12279 -17.365
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
-0.173648 0.984808 0 0
|
||||
-0.696364 -0.122788 -0.707107 0
|
||||
-0.696364 -0.122788 0.707107 0
|
||||
98.4808 17.3648 100 1
|
||||
-0.17365 0.69636 -0.69636 98.481
|
||||
0.98481 0.12279 -0.12279 17.365
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
-0.5 0.866025 0 0
|
||||
-0.612372 -0.353553 -0.707107 0
|
||||
-0.612372 -0.353553 0.707107 0
|
||||
86.6025 50 100 1
|
||||
-0.5 0.61237 -0.61237 86.603
|
||||
0.86603 0.35355 -0.35355 50
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
-0.766044 0.642788 0 0
|
||||
-0.454519 -0.541675 -0.707107 0
|
||||
-0.454519 -0.541675 0.707107 0
|
||||
64.2788 76.6044 100 1
|
||||
-0.76604 0.45452 -0.45452 64.279
|
||||
0.64279 0.54168 -0.54168 76.604
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
-0.939693 0.34202 0 0
|
||||
-0.241845 -0.664463 -0.707107 0
|
||||
-0.241845 -0.664463 0.707107 0
|
||||
34.202 93.9693 100 1
|
||||
-0.93969 0.24185 -0.24185 34.202
|
||||
0.34202 0.66446 -0.66446 93.969
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
-1 0 0 0
|
||||
0 -0.707107 -0.707107 0
|
||||
0 -0.707107 0.707107 0
|
||||
0 100 100 1
|
||||
-1 -0 0 0
|
||||
0 0.70711 -0.70711 100
|
||||
0 -0.70711 -0.70711 100
|
||||
0 0 0 1
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
*/
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
using namespace boost;
|
||||
|
||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
||||
|
|
@ -40,8 +41,8 @@ using namespace boost;
|
|||
/* ************************************************************************* */
|
||||
#define CALIB_FILE "calib.txt"
|
||||
#define LANDMARKS_FILE "landmarks.txt"
|
||||
#define POSES_FILE "posesISAM.txt"
|
||||
#define MEASUREMENTS_FILE "measurementsISAM.txt"
|
||||
#define POSES_FILE "poses.txt"
|
||||
#define MEASUREMENTS_FILE "measurements.txt"
|
||||
|
||||
// Base data folder
|
||||
string g_dataFolder;
|
||||
|
|
@ -49,8 +50,8 @@ string g_dataFolder;
|
|||
// Store groundtruth values, read from files
|
||||
shared_ptrK g_calib;
|
||||
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
|
||||
std::vector<Pose3> g_poses; // prior camera poses at each frame
|
||||
std::vector<std::vector<Feature2D> > g_measurements; // feature sets detected at each frame
|
||||
map<int, Pose3> g_poses; // map: <camera_id, pose>
|
||||
std::map<int, std::vector<Feature2D> > g_measurements; // feature sets detected at each frame
|
||||
|
||||
// Noise models
|
||||
SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
|
||||
|
|
@ -69,7 +70,7 @@ void readAllDataISAM() {
|
|||
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
|
||||
|
||||
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
|
||||
g_poses = readPosesISAM(g_dataFolder, POSES_FILE);
|
||||
g_poses = readPoses(g_dataFolder, POSES_FILE);
|
||||
|
||||
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
|
||||
g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
|
||||
|
|
@ -80,7 +81,7 @@ void readAllDataISAM() {
|
|||
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
|
||||
*/
|
||||
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
|
||||
int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
|
||||
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
|
||||
|
||||
// Create a graph of newFactors with new measurements
|
||||
newFactors = shared_ptr<Graph> (new Graph());
|
||||
|
|
@ -121,16 +122,19 @@ int main(int argc, char* argv[]) {
|
|||
g_dataFolder = string(argv[1]) + "/";
|
||||
readAllDataISAM();
|
||||
|
||||
// Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
|
||||
// Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
|
||||
int relinearizeInterval = 3;
|
||||
NonlinearISAM<visualSLAM::Values> isam(relinearizeInterval);
|
||||
|
||||
// At each frame i with new camera pose and new set of measurements associated with it,
|
||||
// At each frame (poseId) with new camera pose and set of associated measurements,
|
||||
// create a graph of new factors and update ISAM
|
||||
for (size_t i = 0; i < g_measurements.size(); i++) {
|
||||
typedef std::map<int, std::vector<Feature2D> > FeatureMap;
|
||||
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
|
||||
const int poseId = features.first;
|
||||
shared_ptr<Graph> newFactors;
|
||||
shared_ptr<visualSLAM::Values> initialValues;
|
||||
createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib);
|
||||
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
|
||||
features.second, measurementSigma, g_calib);
|
||||
|
||||
isam.update(*newFactors, *initialValues);
|
||||
visualSLAM::Values currentEstimate = isam.estimate();
|
||||
|
|
|
|||
|
|
@ -46,10 +46,6 @@ std::map<int, Point3> readLandMarks(const std::string& landmarkFile) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Read pose from file, output by Panda3D.
|
||||
* Warning: row major!!!
|
||||
*/
|
||||
gtsam::Pose3 readPose(const char* Fn) {
|
||||
ifstream poseFile(Fn);
|
||||
if (!poseFile) {
|
||||
|
|
@ -62,18 +58,7 @@ gtsam::Pose3 readPose(const char* Fn) {
|
|||
poseFile >> v[i];
|
||||
poseFile.close();
|
||||
|
||||
// Because panda3d's camera is z-up, y-view,
|
||||
// we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule
|
||||
//... similar to OpenGL's camera
|
||||
for (int i = 0; i<3; i++) {
|
||||
float t = v[4+i];
|
||||
v[4+i] = v[8+i];
|
||||
v[8+i] = -t;
|
||||
}
|
||||
|
||||
::Vector vec = Vector_(16, v);
|
||||
|
||||
Matrix T = Matrix_(4,4, vec); // column order !!!
|
||||
Matrix T = Matrix_(4,4, v); // row order !!!
|
||||
|
||||
Pose3 pose(T);
|
||||
return pose;
|
||||
|
|
@ -166,28 +151,7 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn) {
|
||||
ifstream posesFile((baseFolder+posesFn).c_str());
|
||||
if (!posesFile) {
|
||||
cout << "Cannot read all pose ISAM file: " << posesFn << endl;
|
||||
exit(0);
|
||||
}
|
||||
int numPoses;
|
||||
posesFile >> numPoses;
|
||||
vector<Pose3> poses;
|
||||
for (int i = 0; i<numPoses; i++) {
|
||||
string poseFileName;
|
||||
posesFile >> poseFileName;
|
||||
|
||||
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
|
||||
poses.push_back(pose);
|
||||
}
|
||||
|
||||
return poses;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
|
||||
std::map<int, std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
|
||||
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
|
||||
if (!measurementsFile) {
|
||||
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
|
||||
|
|
@ -196,13 +160,16 @@ std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string&
|
|||
int numPoses;
|
||||
measurementsFile >> numPoses;
|
||||
|
||||
std::vector<std::vector<Feature2D> > allFeatures;
|
||||
std::map<int, std::vector<Feature2D> > allFeatures;
|
||||
|
||||
for (int i = 0; i<numPoses; i++) {
|
||||
int poseId;
|
||||
measurementsFile >> poseId;
|
||||
|
||||
string featureFileName;
|
||||
measurementsFile >> featureFileName;
|
||||
vector<Feature2D> features = readFeatures(-1, (baseFolder+featureFileName).c_str()); // we don't care about pose id in ISAM
|
||||
allFeatures.push_back(features);
|
||||
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
|
||||
allFeatures[poseId] = features;
|
||||
}
|
||||
|
||||
return allFeatures;
|
||||
|
|
|
|||
|
|
@ -29,11 +29,9 @@ std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile);
|
|||
|
||||
gtsam::Pose3 readPose(const char* poseFn);
|
||||
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFN);
|
||||
std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFN);
|
||||
|
||||
|
||||
gtsam::shared_ptrK readCalibData(const std::string& calibFn);
|
||||
|
||||
std::vector<Feature2D> readFeatureFile(const char* filename);
|
||||
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
|
||||
std::vector< std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);
|
||||
std::map<int, std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);
|
||||
|
|
|
|||
182
gtsam-broken.h
182
gtsam-broken.h
|
|
@ -1,182 +0,0 @@
|
|||
// These are considered to be broken and will be added back as they start working
|
||||
// It's assumed that there have been interface changes that might break this
|
||||
|
||||
class Ordering{
|
||||
Ordering(string key);
|
||||
void print(string s) const;
|
||||
bool equals(const Ordering& ord, double tol) const;
|
||||
Ordering subtract(const Ordering& keys) const;
|
||||
void unique ();
|
||||
void reverse ();
|
||||
void push_back(string s);
|
||||
};
|
||||
|
||||
class GaussianFactorSet {
|
||||
GaussianFactorSet();
|
||||
void push_back(GaussianFactor* factor);
|
||||
};
|
||||
|
||||
class Simulated2DValues {
|
||||
Simulated2DValues();
|
||||
void print(string s) const;
|
||||
void insertPose(int i, const Point2& p);
|
||||
void insertPoint(int j, const Point2& p);
|
||||
int nrPoses() const;
|
||||
int nrPoints() const;
|
||||
Point2* pose(int i);
|
||||
Point2* point(int j);
|
||||
};
|
||||
|
||||
class Simulated2DOrientedValues {
|
||||
Simulated2DOrientedValues();
|
||||
void print(string s) const;
|
||||
void insertPose(int i, const Pose2& p);
|
||||
void insertPoint(int j, const Point2& p);
|
||||
int nrPoses() const;
|
||||
int nrPoints() const;
|
||||
Pose2* pose(int i);
|
||||
Point2* point(int j);
|
||||
};
|
||||
|
||||
class Simulated2DPosePrior {
|
||||
Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
|
||||
void print(string s) const;
|
||||
double error(const Simulated2DValues& c) const;
|
||||
};
|
||||
|
||||
class Simulated2DOrientedPosePrior {
|
||||
Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i);
|
||||
void print(string s) const;
|
||||
double error(const Simulated2DOrientedValues& c) const;
|
||||
};
|
||||
|
||||
class Simulated2DPointPrior {
|
||||
Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
|
||||
void print(string s) const;
|
||||
double error(const Simulated2DValues& c) const;
|
||||
};
|
||||
|
||||
class Simulated2DOdometry {
|
||||
Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2);
|
||||
void print(string s) const;
|
||||
double error(const Simulated2DValues& c) const;
|
||||
};
|
||||
|
||||
class Simulated2DOrientedOdometry {
|
||||
Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2);
|
||||
void print(string s) const;
|
||||
double error(const Simulated2DOrientedValues& c) const;
|
||||
};
|
||||
|
||||
class Simulated2DMeasurement {
|
||||
Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
|
||||
void print(string s) const;
|
||||
double error(const Simulated2DValues& c) const;
|
||||
};
|
||||
|
||||
// These are currently broken
|
||||
// Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class
|
||||
// We also have to solve the shared pointer mess to avoid duplicate methods
|
||||
|
||||
class GaussianFactor {
|
||||
GaussianFactor(string key1,
|
||||
Matrix A1,
|
||||
Vector b_in,
|
||||
const SharedDiagonal& model);
|
||||
GaussianFactor(string key1,
|
||||
Matrix A1,
|
||||
string key2,
|
||||
Matrix A2,
|
||||
Vector b_in,
|
||||
const SharedDiagonal& model);
|
||||
GaussianFactor(string key1,
|
||||
Matrix A1,
|
||||
string key2,
|
||||
Matrix A2,
|
||||
string key3,
|
||||
Matrix A3,
|
||||
Vector b_in,
|
||||
const SharedDiagonal& model);
|
||||
bool involves(string key) const;
|
||||
Matrix getA(string key) const;
|
||||
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
|
||||
pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
|
||||
};
|
||||
|
||||
class GaussianFactorGraph {
|
||||
GaussianConditional* eliminateOne(string key);
|
||||
GaussianBayesNet* eliminate_(const Ordering& ordering);
|
||||
VectorValues* optimize_(const Ordering& ordering);
|
||||
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
|
||||
VectorValues* steepestDescent_(const VectorValues& x0) const;
|
||||
VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
|
||||
};
|
||||
|
||||
|
||||
class Pose2Values{
|
||||
Pose2Values();
|
||||
Pose2 get(string key) const;
|
||||
void insert(string name, const Pose2& val);
|
||||
void print(string s) const;
|
||||
void clear();
|
||||
int size();
|
||||
};
|
||||
|
||||
class Pose2Factor {
|
||||
Pose2Factor(string key1, string key2,
|
||||
const Pose2& measured, Matrix measurement_covariance);
|
||||
void print(string name) const;
|
||||
double error(const Pose2Values& c) const;
|
||||
size_t size() const;
|
||||
GaussianFactor* linearize(const Pose2Values& config) const;
|
||||
};
|
||||
|
||||
class Pose2Graph{
|
||||
Pose2Graph();
|
||||
void print(string s) const;
|
||||
GaussianFactorGraph* linearize_(const Pose2Values& config) const;
|
||||
void push_back(Pose2Factor* factor);
|
||||
};
|
||||
|
||||
class SymbolicFactor{
|
||||
SymbolicFactor(const Ordering& keys);
|
||||
void print(string s) const;
|
||||
};
|
||||
|
||||
class Simulated2DPosePrior {
|
||||
GaussianFactor* linearize(const Simulated2DValues& config) const;
|
||||
};
|
||||
|
||||
class Simulated2DOrientedPosePrior {
|
||||
GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
|
||||
};
|
||||
|
||||
class Simulated2DPointPrior {
|
||||
GaussianFactor* linearize(const Simulated2DValues& config) const;
|
||||
};
|
||||
|
||||
class Simulated2DOdometry {
|
||||
GaussianFactor* linearize(const Simulated2DValues& config) const;
|
||||
};
|
||||
|
||||
class Simulated2DOrientedOdometry {
|
||||
GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
|
||||
};
|
||||
|
||||
class Simulated2DMeasurement {
|
||||
GaussianFactor* linearize(const Simulated2DValues& config) const;
|
||||
};
|
||||
|
||||
class Pose2SLAMOptimizer {
|
||||
Pose2SLAMOptimizer(string dataset_name);
|
||||
void print(string s) const;
|
||||
void update(Vector x) const;
|
||||
Vector optimize() const;
|
||||
double error() const;
|
||||
Matrix a1() const;
|
||||
Matrix a2() const;
|
||||
Vector b1() const;
|
||||
Vector b2() const;
|
||||
};
|
||||
|
||||
|
||||
399
gtsam.h
399
gtsam.h
|
|
@ -1,29 +1,139 @@
|
|||
/**
|
||||
* GTSAM Wrap Module Definition
|
||||
*
|
||||
* These are the current classes available through the matlab toolbox interface,
|
||||
* add more functions/classes as they are available.
|
||||
*
|
||||
* Requirements:
|
||||
* Classes must start with an uppercase letter
|
||||
* Only one Method/Constructor per line
|
||||
* Methods can return
|
||||
* - Eigen types: Matrix, Vector
|
||||
* - C/C++ basic types: string, bool, size_t, int, double
|
||||
* - void
|
||||
* - Any class with which be copied with boost::make_shared()
|
||||
* - boost::shared_ptr of any object type
|
||||
* Limitations on methods
|
||||
* - Parsing does not support overloading
|
||||
* - There can only be one method with a given name
|
||||
* Arguments to functions any of
|
||||
* - Eigen types: Matrix, Vector
|
||||
* - Eigen types and classes as an optionally const reference
|
||||
* - C/C++ basic types: string, bool, size_t, int, double
|
||||
* - Any class with which be copied with boost::make_shared() (except Eigen)
|
||||
* - boost::shared_ptr of any object type (except Eigen)
|
||||
* Comments can use either C++ or C style, with multiple lines
|
||||
* Namespace definitions
|
||||
* - Names of namespaces must start with a lowercase letter
|
||||
* - start a namespace with "namespace {"
|
||||
* - end a namespace with exactly "}///\namespace [namespace_name]", optionally adding the name of the namespace
|
||||
* - This ending is not C++ standard, and must contain "}///\namespace" to parse
|
||||
* - Namespaces can be nested
|
||||
* Namespace usage
|
||||
* - Namespaces can be specified for classes in arguments and return values
|
||||
* - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName"
|
||||
* Methods must start with a lowercase letter
|
||||
* Static methods must start with a letter (upper or lowercase) and use the "static" keyword
|
||||
* Includes in C++ wrappers
|
||||
* - By default, the include will be <[classname].h>
|
||||
* - To override, add a full include statement inside the class definition
|
||||
*/
|
||||
|
||||
/**
|
||||
* Status:
|
||||
* - TODO: global functions
|
||||
* - TODO: default values for arguments
|
||||
* - TODO: overloaded functions
|
||||
* - TODO: Handle Rot3M conversions to quaternions
|
||||
*/
|
||||
|
||||
// Everything is in the gtsam namespace, so we avoid copying everything in
|
||||
using namespace gtsam;
|
||||
|
||||
class Point2 {
|
||||
Point2();
|
||||
Point2(double x, double y);
|
||||
static Point2 Expmap(Vector v);
|
||||
static Vector Logmap(const Point2& p);
|
||||
void print(string s) const;
|
||||
double x();
|
||||
double y();
|
||||
Vector localCoordinates(const Point2& p);
|
||||
Point2 compose(const Point2& p2);
|
||||
Point2 between(const Point2& p2);
|
||||
Point2 retract(Vector v);
|
||||
};
|
||||
|
||||
class Point3 {
|
||||
Point3();
|
||||
Point3(double x, double y, double z);
|
||||
Point3(Vector v);
|
||||
static Point3 Expmap(Vector v);
|
||||
static Vector Logmap(const Point3& p);
|
||||
void print(string s) const;
|
||||
bool equals(const Point3& p, double tol);
|
||||
Vector vector() const;
|
||||
double x();
|
||||
double y();
|
||||
double z();
|
||||
Vector localCoordinates(const Point3& p);
|
||||
Point3 retract(Vector v);
|
||||
Point3 compose(const Point3& p2);
|
||||
Point3 between(const Point3& p2);
|
||||
};
|
||||
|
||||
class Rot2 {
|
||||
Rot2();
|
||||
Rot2(double theta);
|
||||
static Rot2 Expmap(Vector v);
|
||||
static Vector Logmap(const Rot2& p);
|
||||
static Rot2 fromAngle(double theta);
|
||||
static Rot2 fromDegrees(double theta);
|
||||
static Rot2 fromCosSin(double c, double s);
|
||||
static Rot2 relativeBearing(const Point2& d); // Ignoring derivative
|
||||
static Rot2 atan2(double y, double x);
|
||||
void print(string s) const;
|
||||
bool equals(const Rot2& pose, double tol) const;
|
||||
bool equals(const Rot2& rot, double tol) const;
|
||||
double theta() const;
|
||||
double degrees() const;
|
||||
double c() const;
|
||||
double s() const;
|
||||
Vector localCoordinates(const Rot2& p);
|
||||
Rot2 retract(Vector v);
|
||||
Rot2 compose(const Rot2& p2);
|
||||
Rot2 between(const Rot2& p2);
|
||||
};
|
||||
|
||||
class Rot3 {
|
||||
Rot3();
|
||||
Rot3(Matrix R);
|
||||
static Rot3 Expmap(Vector v);
|
||||
static Vector Logmap(const Rot3& p);
|
||||
static Rot3 ypr(double y, double p, double r);
|
||||
static Rot3 Rx(double t);
|
||||
static Rot3 Ry(double t);
|
||||
static Rot3 Rz(double t);
|
||||
static Rot3 RzRyRx(double x, double y, double z);
|
||||
static Rot3 RzRyRx(const Vector& xyz);
|
||||
static Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading)
|
||||
static Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
|
||||
static Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft)
|
||||
static Rot3 quaternion(double w, double x, double y, double z);
|
||||
static Rot3 rodriguez(const Vector& v);
|
||||
Matrix matrix() const;
|
||||
Matrix transpose() const;
|
||||
Vector xyz() const;
|
||||
Vector ypr() const;
|
||||
double roll() const;
|
||||
double pitch() const;
|
||||
double yaw() const;
|
||||
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
|
||||
void print(string s) const;
|
||||
bool equals(const Rot3& rot, double tol) const;
|
||||
Vector localCoordinates(const Rot3& p);
|
||||
Rot3 retract(Vector v);
|
||||
Rot3 compose(const Rot3& p2);
|
||||
Rot3 between(const Rot3& p2);
|
||||
};
|
||||
|
||||
class Pose2 {
|
||||
|
|
@ -32,15 +142,41 @@ class Pose2 {
|
|||
Pose2(double theta, const Point2& t);
|
||||
Pose2(const Rot2& r, const Point2& t);
|
||||
Pose2(Vector v);
|
||||
static Pose2 Expmap(Vector v);
|
||||
static Vector Logmap(const Pose2& p);
|
||||
void print(string s) const;
|
||||
bool equals(const Pose2& pose, double tol) const;
|
||||
double x() const;
|
||||
double y() const;
|
||||
double theta() const;
|
||||
int dim() const;
|
||||
Pose2* compose_(const Pose2& p2);
|
||||
Pose2* between_(const Pose2& p2);
|
||||
Vector localCoordinates(const Pose2& p);
|
||||
Pose2 retract(Vector v);
|
||||
Pose2 compose(const Pose2& p2);
|
||||
Pose2 between(const Pose2& p2);
|
||||
Rot2 bearing(const Point2& point);
|
||||
double range(const Point2& point);
|
||||
};
|
||||
|
||||
class Pose3 {
|
||||
Pose3();
|
||||
Pose3(const Rot3& r, const Point3& t);
|
||||
Pose3(Vector v);
|
||||
Pose3(Matrix t);
|
||||
static Pose3 Expmap(Vector v);
|
||||
static Vector Logmap(const Pose3& p);
|
||||
void print(string s) const;
|
||||
bool equals(const Pose3& pose, double tol) const;
|
||||
double x() const;
|
||||
double y() const;
|
||||
double z() const;
|
||||
Matrix matrix() const;
|
||||
Matrix adjointMap() const;
|
||||
Pose3 compose(const Pose3& p2);
|
||||
Pose3 between(const Pose3& p2);
|
||||
Pose3 retract(Vector v);
|
||||
Point3 translation() const;
|
||||
Rot3 rotation() const;
|
||||
};
|
||||
|
||||
class SharedGaussian {
|
||||
|
|
@ -54,6 +190,12 @@ class SharedDiagonal {
|
|||
Vector sample() const;
|
||||
};
|
||||
|
||||
class SharedNoiseModel {
|
||||
#include <gtsam/linear/SharedGaussian.h>
|
||||
SharedNoiseModel(const SharedDiagonal& model);
|
||||
SharedNoiseModel(const SharedGaussian& model);
|
||||
};
|
||||
|
||||
class VectorValues {
|
||||
VectorValues();
|
||||
VectorValues(int nVars, int varDim);
|
||||
|
|
@ -118,6 +260,14 @@ class GaussianFactorGraph {
|
|||
Matrix sparseJacobian_() const;
|
||||
};
|
||||
|
||||
class GaussianSequentialSolver {
|
||||
GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR);
|
||||
GaussianBayesNet* eliminate() const;
|
||||
VectorValues* optimize() const;
|
||||
GaussianFactor* marginalFactor(int j) const;
|
||||
Matrix marginalCovariance(int j) const;
|
||||
};
|
||||
|
||||
class KalmanFilter {
|
||||
KalmanFilter(Vector x, const SharedDiagonal& model);
|
||||
void print(string s) const;
|
||||
|
|
@ -129,14 +279,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;
|
||||
|
|
@ -144,22 +286,28 @@ class Ordering {
|
|||
void push_back(string key);
|
||||
};
|
||||
|
||||
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);
|
||||
|
|
@ -169,18 +317,215 @@ 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;
|
||||
};
|
||||
|
||||
class GaussianSequentialSolver {
|
||||
GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR);
|
||||
GaussianBayesNet* eliminate();
|
||||
VectorValues* optimize();
|
||||
}///\namespace planarSLAM
|
||||
|
||||
// Simulated2D Example Domain
|
||||
namespace simulated2D {
|
||||
|
||||
class Values {
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
Values();
|
||||
void insertPose(int i, const Point2& p);
|
||||
void insertPoint(int j, const Point2& p);
|
||||
int nrPoses() const;
|
||||
int nrPoints() const;
|
||||
Point2 pose(int i);
|
||||
Point2 point(int j);
|
||||
};
|
||||
|
||||
class Graph {
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
Graph();
|
||||
};
|
||||
|
||||
// TODO: add factors, etc.
|
||||
|
||||
}///\namespace simulated2D
|
||||
|
||||
// Simulated2DOriented Example Domain
|
||||
namespace simulated2DOriented {
|
||||
|
||||
class Values {
|
||||
#include <gtsam/slam/simulated2DOriented.h>
|
||||
Values();
|
||||
void insertPose(int i, const Pose2& p);
|
||||
void insertPoint(int j, const Point2& p);
|
||||
int nrPoses() const;
|
||||
int nrPoints() const;
|
||||
Pose2 pose(int i);
|
||||
Point2 point(int j);
|
||||
};
|
||||
|
||||
class Graph {
|
||||
#include <gtsam/slam/simulated2DOriented.h>
|
||||
Graph();
|
||||
};
|
||||
|
||||
// TODO: add factors, etc.
|
||||
|
||||
}///\namespace simulated2DOriented
|
||||
|
||||
//// These are considered to be broken and will be added back as they start working
|
||||
//// It's assumed that there have been interface changes that might break this
|
||||
//
|
||||
//class Ordering{
|
||||
// Ordering(string key);
|
||||
// void print(string s) const;
|
||||
// bool equals(const Ordering& ord, double tol) const;
|
||||
// Ordering subtract(const Ordering& keys) const;
|
||||
// void unique ();
|
||||
// void reverse ();
|
||||
// void push_back(string s);
|
||||
//};
|
||||
//
|
||||
//class GaussianFactorSet {
|
||||
// GaussianFactorSet();
|
||||
// void push_back(GaussianFactor* factor);
|
||||
//};
|
||||
//
|
||||
//class Simulated2DPosePrior {
|
||||
// Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
|
||||
// void print(string s) const;
|
||||
// double error(const Simulated2DValues& c) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DOrientedPosePrior {
|
||||
// Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i);
|
||||
// void print(string s) const;
|
||||
// double error(const Simulated2DOrientedValues& c) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DPointPrior {
|
||||
// Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
|
||||
// void print(string s) const;
|
||||
// double error(const Simulated2DValues& c) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DOdometry {
|
||||
// Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2);
|
||||
// void print(string s) const;
|
||||
// double error(const Simulated2DValues& c) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DOrientedOdometry {
|
||||
// Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2);
|
||||
// void print(string s) const;
|
||||
// double error(const Simulated2DOrientedValues& c) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DMeasurement {
|
||||
// Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
|
||||
// void print(string s) const;
|
||||
// double error(const Simulated2DValues& c) const;
|
||||
//};
|
||||
//
|
||||
//class GaussianFactor {
|
||||
// GaussianFactor(string key1,
|
||||
// Matrix A1,
|
||||
// Vector b_in,
|
||||
// const SharedDiagonal& model);
|
||||
// GaussianFactor(string key1,
|
||||
// Matrix A1,
|
||||
// string key2,
|
||||
// Matrix A2,
|
||||
// Vector b_in,
|
||||
// const SharedDiagonal& model);
|
||||
// GaussianFactor(string key1,
|
||||
// Matrix A1,
|
||||
// string key2,
|
||||
// Matrix A2,
|
||||
// string key3,
|
||||
// Matrix A3,
|
||||
// Vector b_in,
|
||||
// const SharedDiagonal& model);
|
||||
// bool involves(string key) const;
|
||||
// Matrix getA(string key) const;
|
||||
// pair<Matrix,Vector> matrix(const Ordering& ordering) const;
|
||||
// pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
|
||||
//};
|
||||
//
|
||||
//class GaussianFactorGraph {
|
||||
// GaussianConditional* eliminateOne(string key);
|
||||
// GaussianBayesNet* eliminate_(const Ordering& ordering);
|
||||
// VectorValues* optimize_(const Ordering& ordering);
|
||||
// pair<Matrix,Vector> matrix(const Ordering& ordering) const;
|
||||
// VectorValues* steepestDescent_(const VectorValues& x0) const;
|
||||
// VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
|
||||
//};
|
||||
//
|
||||
//class Pose2Values{
|
||||
// Pose2Values();
|
||||
// Pose2 get(string key) const;
|
||||
// void insert(string name, const Pose2& val);
|
||||
// void print(string s) const;
|
||||
// void clear();
|
||||
// int size();
|
||||
//};
|
||||
//
|
||||
//class Pose2Factor {
|
||||
// Pose2Factor(string key1, string key2,
|
||||
// const Pose2& measured, Matrix measurement_covariance);
|
||||
// void print(string name) const;
|
||||
// double error(const Pose2Values& c) const;
|
||||
// size_t size() const;
|
||||
// GaussianFactor* linearize(const Pose2Values& config) const;
|
||||
//};
|
||||
//
|
||||
//class Pose2Graph{
|
||||
// Pose2Graph();
|
||||
// void print(string s) const;
|
||||
// GaussianFactorGraph* linearize_(const Pose2Values& config) const;
|
||||
// void push_back(Pose2Factor* factor);
|
||||
//};
|
||||
//
|
||||
//class SymbolicFactor{
|
||||
// SymbolicFactor(const Ordering& keys);
|
||||
// void print(string s) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DPosePrior {
|
||||
// GaussianFactor* linearize(const Simulated2DValues& config) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DOrientedPosePrior {
|
||||
// GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DPointPrior {
|
||||
// GaussianFactor* linearize(const Simulated2DValues& config) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DOdometry {
|
||||
// GaussianFactor* linearize(const Simulated2DValues& config) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DOrientedOdometry {
|
||||
// GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
|
||||
//};
|
||||
//
|
||||
//class Simulated2DMeasurement {
|
||||
// GaussianFactor* linearize(const Simulated2DValues& config) const;
|
||||
//};
|
||||
//
|
||||
//class Pose2SLAMOptimizer {
|
||||
// Pose2SLAMOptimizer(string dataset_name);
|
||||
// void print(string s) const;
|
||||
// void update(Vector x) const;
|
||||
// Vector optimize() const;
|
||||
// double error() const;
|
||||
// Matrix a1() const;
|
||||
// Matrix a2() const;
|
||||
// Vector b1() const;
|
||||
// Vector b2() const;
|
||||
//};
|
||||
|
|
|
|||
|
|
@ -277,6 +277,18 @@ bool assert_container_equality(const V& expected, const V& actual) {
|
|||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compare strings for unit tests
|
||||
*/
|
||||
bool assert_equal(const std::string& expected, const std::string& actual) {
|
||||
if (expected == actual)
|
||||
return true;
|
||||
printf("Not equal:\n");
|
||||
std::cout << "expected: [" << expected << "]\n";
|
||||
std::cout << "actual: [" << actual << "]" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Allow for testing inequality
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -164,13 +164,14 @@ Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal)
|
|||
|
||||
if(debug) {
|
||||
cout << "Partial LDL with " << nFrontal << " frontal scalars, ";
|
||||
print(ABC, "ABC: ");
|
||||
print(ABC, "LDL ABC: ");
|
||||
}
|
||||
|
||||
// Compute Cholesky factorization of A, overwrites A = sqrt(D)*R
|
||||
// tic(1, "ldl");
|
||||
Eigen::LDLT<Matrix> ldlt;
|
||||
ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>());
|
||||
if (debug) ldlt.isNegative() ? cout << "Matrix is negative" << endl : cout << "Matrix is not negative" << endl;
|
||||
|
||||
if(ldlt.vectorD().unaryExpr(boost::bind(less<double>(), _1, 0.0)).any()) {
|
||||
if(ISDEBUG("detailed_exceptions"))
|
||||
|
|
@ -180,14 +181,15 @@ Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal)
|
|||
}
|
||||
|
||||
Vector sqrtD = ldlt.vectorD().cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL
|
||||
if (debug) cout << "Dsqrt: " << sqrtD << endl;
|
||||
if (debug) cout << "LDL Dsqrt:\n" << sqrtD << endl;
|
||||
|
||||
// U = sqrtD * L^
|
||||
Matrix U = ldlt.matrixU();
|
||||
if(debug) cout << "LDL U:\n" << U << endl;
|
||||
|
||||
// we store the permuted upper triangular matrix
|
||||
ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; // FIXME: this isn't actually LDL', it's Cholesky
|
||||
if(debug) cout << "R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl;
|
||||
if(debug) cout << "LDL R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl;
|
||||
// toc(1, "ldl");
|
||||
|
||||
// Compute S = inv(R') * B = inv(P'U')*B = inv(U')*P*B
|
||||
|
|
@ -196,20 +198,19 @@ Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal)
|
|||
ABC.topRightCorner(nFrontal, n-nFrontal) = ldlt.transpositionsP() * ABC.topRightCorner(nFrontal, n-nFrontal);
|
||||
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(ABC.topRightCorner(nFrontal, n-nFrontal));
|
||||
}
|
||||
if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
|
||||
if(debug) cout << "LDL S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
|
||||
// toc(2, "compute S");
|
||||
|
||||
// Compute L = C - S' * S
|
||||
// tic(3, "compute L");
|
||||
if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
|
||||
if(debug) cout << "LDL C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
|
||||
if(n - nFrontal > 0)
|
||||
ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
|
||||
ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
|
||||
if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
|
||||
if(debug) cout << "LDL L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
|
||||
// toc(3, "compute L");
|
||||
|
||||
if(debug) cout << "P: " << ldlt.transpositionsP().indices() << endl;
|
||||
|
||||
if(debug) cout << "LDL P: " << ldlt.transpositionsP().indices() << endl;
|
||||
return ldlt.transpositionsP();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -103,11 +103,6 @@ public:
|
|||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/// MATLAB version returns shared pointer
|
||||
boost::shared_ptr<Pose2> compose_(const Pose2& p2) {
|
||||
return boost::shared_ptr<Pose2>(new Pose2(compose(p2)));
|
||||
}
|
||||
|
||||
/// compose syntactic sugar
|
||||
inline Pose2 operator*(const Pose2& p2) const {
|
||||
return Pose2(r_*p2.r(), t_ + r_*p2.t());
|
||||
|
|
@ -151,11 +146,6 @@ public:
|
|||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// MATLAB version returns shared pointer
|
||||
boost::shared_ptr<Pose2> between_(const Pose2& p2) {
|
||||
return boost::shared_ptr<Pose2>(new Pose2(between(p2)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Return point coordinates in pose coordinate frame
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ namespace gtsam {
|
|||
// Calculate Adjoint map
|
||||
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
|
||||
// Experimental - unit tests of derivatives based on it do not check out yet
|
||||
Matrix Pose3::AdjointMap() const {
|
||||
Matrix Pose3::adjointMap() const {
|
||||
const Matrix R = R_.matrix();
|
||||
const Vector t = t_.vector();
|
||||
Matrix A = skewSymmetric(t)*R;
|
||||
|
|
@ -188,7 +188,7 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) {
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
*H1 = AdjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface
|
||||
*H1 = adjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface
|
||||
#else
|
||||
const Rot3& R2 = p2.rotation();
|
||||
const Point3& t2 = p2.translation();
|
||||
|
|
@ -214,22 +214,23 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1)
|
||||
if (H1)
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
{ *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ?
|
||||
// FIXME: this function doesn't exist with this interface - should this be "*H1 = -adjointMap();" ?
|
||||
{ *H1 = - adjointMap(p); }
|
||||
#else
|
||||
{
|
||||
Matrix Rt = R_.transpose();
|
||||
Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3;
|
||||
Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt;
|
||||
Matrix DR = collect(2, &DR_R1, &DR_t1);
|
||||
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
|
||||
*H1 = gtsam::stack(2, &DR, &Dt);
|
||||
}
|
||||
{
|
||||
Matrix Rt = R_.transpose();
|
||||
Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3;
|
||||
Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt;
|
||||
Matrix DR = collect(2, &DR_R1, &DR_t1);
|
||||
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
|
||||
*H1 = gtsam::stack(2, &DR, &Dt);
|
||||
}
|
||||
#endif
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt*(-t_));
|
||||
}
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt*(-t_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// between = compose(p2,inverse(p1));
|
||||
|
|
|
|||
|
|
@ -99,16 +99,16 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
|
||||
static size_t Dim() { return dimension; }
|
||||
static size_t Dim() { return dimension; }
|
||||
|
||||
/// Dimensionality of the tangent space = 6 DOF
|
||||
size_t dim() const { return dimension; }
|
||||
|
||||
|
||||
/** Exponential map around another pose */ /// Retraction from R^6 to Pose3 manifold neighborhood around current pose
|
||||
/// Retraction from R^6 to Pose3 manifold neighborhood around current pose
|
||||
Pose3 retract(const Vector& d) const;
|
||||
|
||||
/// Logarithm map around another pose T1 /// Local 6D coordinates of Pose3 manifold neighborhood around current pose
|
||||
/// Local 6D coordinates of Pose3 manifold neighborhood around current pose
|
||||
Vector localCoordinates(const Pose3& T2) const;
|
||||
|
||||
/// @}
|
||||
|
|
@ -148,8 +148,8 @@ namespace gtsam {
|
|||
* Calculate Adjoint map
|
||||
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
|
||||
*/
|
||||
Matrix AdjointMap() const;
|
||||
Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
|
||||
Matrix adjointMap() const; /// FIXME Not tested - marked as incorrect
|
||||
Vector adjoint(const Vector& xi) const {return adjointMap()*xi; } /// FIXME Not tested - marked as incorrect
|
||||
|
||||
/**
|
||||
* wedge for Pose3:
|
||||
|
|
|
|||
|
|
@ -25,224 +25,234 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/** Explicit instantiation of base class to export members */
|
||||
INSTANTIATE_LIE(Rot3M);
|
||||
/** Explicit instantiation of base class to export members */
|
||||
INSTANTIATE_LIE(Rot3M);
|
||||
|
||||
static const Matrix I3 = eye(3);
|
||||
static const Matrix I3 = eye(3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// static member functions to construct rotations
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
}
|
||||
|
||||
Rot3M Rot3M::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Ry(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
}
|
||||
|
||||
Rot3M Rot3M::Ry(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Rz(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
ct,-st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
}
|
||||
|
||||
Rot3M Rot3M::Rz(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
ct,-st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
// Considerably faster than composing matrices above !
|
||||
Rot3M Rot3M::RzRyRx(double x, double y, double z) {
|
||||
double cx=cos(x),sx=sin(x);
|
||||
double cy=cos(y),sy=sin(y);
|
||||
double cz=cos(z),sz=sin(z);
|
||||
double ss_ = sx * sy;
|
||||
double cs_ = cx * sy;
|
||||
double sc_ = sx * cy;
|
||||
double cc_ = cx * cy;
|
||||
double c_s = cx * sz;
|
||||
double s_s = sx * sz;
|
||||
double _cs = cy * sz;
|
||||
double _cc = cy * cz;
|
||||
double s_c = sx * cz;
|
||||
double c_c = cx * cz;
|
||||
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
|
||||
return Rot3M(
|
||||
_cc,- c_s + ssc, s_s + csc,
|
||||
_cs, c_c + sss, -s_c + css,
|
||||
-sy, sc_, cc_
|
||||
);
|
||||
}
|
||||
|
||||
// Considerably faster than composing matrices above !
|
||||
Rot3M Rot3M::RzRyRx(double x, double y, double z) {
|
||||
double cx=cos(x),sx=sin(x);
|
||||
double cy=cos(y),sy=sin(y);
|
||||
double cz=cos(z),sz=sin(z);
|
||||
double ss_ = sx * sy;
|
||||
double cs_ = cx * sy;
|
||||
double sc_ = sx * cy;
|
||||
double cc_ = cx * cy;
|
||||
double c_s = cx * sz;
|
||||
double s_s = sx * sz;
|
||||
double _cs = cy * sz;
|
||||
double _cc = cy * cz;
|
||||
double s_c = sx * cz;
|
||||
double c_c = cx * cz;
|
||||
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
|
||||
return Rot3M(
|
||||
_cc,- c_s + ssc, s_s + csc,
|
||||
_cs, c_c + sss, -s_c + css,
|
||||
-sy, sc_, cc_
|
||||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
|
||||
// get components of axis \omega
|
||||
double wx = w(0), wy=w(1), wz=w(2);
|
||||
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
|
||||
// get components of axis \omega
|
||||
double wx = w(0), wy=w(1), wz=w(2);
|
||||
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
|
||||
#ifndef NDEBUG
|
||||
double l_n = wwTxx + wwTyy + wwTzz;
|
||||
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
||||
double l_n = wwTxx + wwTyy + wwTzz;
|
||||
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
||||
#endif
|
||||
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
|
||||
return Rot3M( c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
return Rot3M(
|
||||
c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3M();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3M();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3M::equals(const Rot3M & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
bool Rot3M::equals(const Rot3M & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::matrix() const {
|
||||
double r[] = { r1_.x(), r2_.x(), r3_.x(),
|
||||
r1_.y(), r2_.y(), r3_.y(),
|
||||
r1_.z(), r2_.z(), r3_.z() };
|
||||
return Matrix_(3,3, r);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::matrix() const {
|
||||
Matrix R(3,3);
|
||||
R <<
|
||||
r1_.x(), r2_.x(), r3_.x(),
|
||||
r1_.y(), r2_.y(), r3_.y(),
|
||||
r1_.z(), r2_.z(), r3_.z();
|
||||
return R;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::transpose() const {
|
||||
double r[] = { r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z()};
|
||||
return Matrix_(3,3, r);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::transpose() const {
|
||||
Matrix Rt(3,3);
|
||||
Rt <<
|
||||
r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z();
|
||||
return Rt;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3_;
|
||||
else if (index == 2)
|
||||
return r2_;
|
||||
else
|
||||
return r1_; // default returns r1
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3_;
|
||||
else if (index == 2)
|
||||
return r2_;
|
||||
else
|
||||
return r1_; // default returns r1
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
Vector Rot3M::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
Vector Rot3M::rpy() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(0),q(1),q(2));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::rpy() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(0),q(1),q(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector Rot3M::Logmap(const Rot3M& R) {
|
||||
double tr = R.r1().x()+R.r2().y()+R.r3().z();
|
||||
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
|
||||
return zero(3);
|
||||
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4)
|
||||
return (0.5 + theta*theta/12)*Vector_(3,
|
||||
R.r2().z()-R.r3().y(),
|
||||
R.r3().x()-R.r1().z(),
|
||||
R.r1().y()-R.r2().x());
|
||||
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
|
||||
if(fabs(R.r3().z() - -1.0) > 1e-10)
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
|
||||
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
|
||||
else if(fabs(R.r2().y() - -1.0) > 1e-10)
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
|
||||
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
|
||||
else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
|
||||
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
|
||||
} else {
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
return (theta/2.0/sin(theta))*Vector_(3,
|
||||
R.r2().z()-R.r3().y(),
|
||||
R.r3().x()-R.r1().z(),
|
||||
R.r1().y()-R.r2().x());
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = matrix();
|
||||
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3M::unrotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
const Matrix Rt(transpose());
|
||||
Point3 q(Rt*p.vector()); // q = Rt*p
|
||||
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
|
||||
if (H2) *H2 = Rt;
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::compose (const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::between (const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = -(R2.transpose()*matrix());
|
||||
if (H2) *H2 = I3;
|
||||
return between_default(*this, R2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3M Qx = Rot3M::Rx(-x);
|
||||
Matrix B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3M Qy = Rot3M::Ry(-y);
|
||||
Matrix C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3M Qz = Rot3M::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector Rot3M::Logmap(const Rot3M& R) {
|
||||
double tr = R.r1().x()+R.r2().y()+R.r3().z();
|
||||
// FIXME should tr in statement below be absolute value?
|
||||
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
|
||||
return zero(3);
|
||||
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4)
|
||||
return (0.5 + theta*theta/12)*Vector_(3,
|
||||
R.r2().z()-R.r3().y(),
|
||||
R.r3().x()-R.r1().z(),
|
||||
R.r1().y()-R.r2().x());
|
||||
// FIXME: in statement below, is this the right comparision?
|
||||
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
|
||||
if(fabs(R.r3().z() - -1.0) > 1e-10)
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
|
||||
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
|
||||
else if(fabs(R.r2().y() - -1.0) > 1e-10)
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
|
||||
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
|
||||
else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
|
||||
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
|
||||
} else {
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
return (theta/2.0/sin(theta))*Vector_(3,
|
||||
R.r2().z()-R.r3().y(),
|
||||
R.r3().x()-R.r1().z(),
|
||||
R.r1().y()-R.r2().x());
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = matrix();
|
||||
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3M::unrotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
const Matrix Rt(transpose());
|
||||
Point3 q(Rt*p.vector()); // q = Rt*p
|
||||
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
|
||||
if (H2) *H2 = Rt;
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::compose (const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::between (const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = -(R2.transpose()*matrix());
|
||||
if (H2) *H2 = I3;
|
||||
return between_default(*this, R2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3M Qx = Rot3M::Rx(-x);
|
||||
Matrix B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3M Qy = Rot3M::Ry(-y);
|
||||
Matrix C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3M Qz = Rot3M::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -226,10 +226,20 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* Use RQ to calculate roll-pitch-yaw angle representation
|
||||
* @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r)
|
||||
* @return a vector containing rpy s.t. R = Rot3M::ypr(y,p,r)
|
||||
*/
|
||||
Vector rpy() const;
|
||||
|
||||
/**
|
||||
* Accessors to get to components of angle representations
|
||||
* NOTE: these are not efficient to get to multiple separate parts,
|
||||
* you should instead use xyz() or ypr()
|
||||
* TODO: make this more efficient
|
||||
*/
|
||||
inline double roll() const { return ypr()(2); }
|
||||
inline double pitch() const { return ypr()(1); }
|
||||
inline double yaw() const { return ypr()(0); }
|
||||
|
||||
/** Compute the quaternion representation of this rotation.
|
||||
* @return The quaternion
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ TEST( Pose3, expmap_a)
|
|||
v(0) = 0.3;
|
||||
EXPECT(assert_equal(id.retract(v), Pose3(R, Point3())));
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
|
||||
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
|
||||
#else
|
||||
v(3)=0.2;v(4)=0.7;v(5)=-2;
|
||||
|
|
@ -100,89 +99,6 @@ namespace screw {
|
|||
Pose3 expected(expectedR, expectedT);
|
||||
}
|
||||
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmap_c)
|
||||
{
|
||||
EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
|
||||
EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
|
||||
TEST(Pose3, Adjoint)
|
||||
{
|
||||
Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T);
|
||||
Vector xiprime = Adjoint(T, screw::xi);
|
||||
EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
|
||||
|
||||
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2);
|
||||
Vector xiprime2 = Adjoint(T2, screw::xi);
|
||||
EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
|
||||
|
||||
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3);
|
||||
Vector xiprime3 = Adjoint(T3, screw::xi);
|
||||
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** Agrawal06iros version */
|
||||
Pose3 Agrawal06iros(const Vector& xi) {
|
||||
Vector w = vector_range<const Vector>(xi, range(0,3));
|
||||
Vector v = vector_range<const Vector>(xi, range(3,6));
|
||||
double t = norm_2(w);
|
||||
if (t < 1e-5)
|
||||
return Pose3(Rot3(), expmap<Point3> (v));
|
||||
else {
|
||||
Matrix W = skewSymmetric(w/t);
|
||||
Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
|
||||
return Pose3(Rot3::Expmap (w), expmap<Point3> (A * v));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmaps_galore)
|
||||
{
|
||||
Vector xi; Pose3 actual;
|
||||
xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
|
||||
actual = Pose3::Expmap(xi);
|
||||
EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
|
||||
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
|
||||
EXPECT(assert_equal(xi, localCoordinates(actual),1e-6));
|
||||
|
||||
xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6);
|
||||
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
|
||||
Vector txi = xi*theta;
|
||||
actual = Pose3::Expmap(txi);
|
||||
EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
|
||||
EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
|
||||
Vector log = localCoordinates(actual);
|
||||
EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6));
|
||||
EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
|
||||
}
|
||||
|
||||
// Works with large v as well, but expm needs 10 iterations!
|
||||
xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0);
|
||||
actual = Pose3::Expmap(xi);
|
||||
EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
|
||||
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
|
||||
EXPECT(assert_equal(xi, localCoordinates(actual),1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, Adjoint_compose)
|
||||
{
|
||||
// To debug derivatives of compose, assert that
|
||||
// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
|
||||
const Pose3& T1 = T;
|
||||
Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8);
|
||||
Pose3 expected = T1 * Pose3::Expmap(x) * T2;
|
||||
Vector y = Adjoint(inverse(T2), x);
|
||||
Pose3 actual = T1 * T2 * Pose3::Expmap(y);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
}
|
||||
#endif // SLOW_BUT_CORRECT_EXMAP
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmap_c_full)
|
||||
{
|
||||
|
|
@ -195,15 +111,15 @@ TEST(Pose3, expmap_c_full)
|
|||
TEST(Pose3, Adjoint_full)
|
||||
{
|
||||
Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse();
|
||||
Vector xiprime = T.Adjoint(screw::xi);
|
||||
Vector xiprime = T.adjoint(screw::xi);
|
||||
EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
|
||||
|
||||
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse();
|
||||
Vector xiprime2 = T2.Adjoint(screw::xi);
|
||||
Vector xiprime2 = T2.adjoint(screw::xi);
|
||||
EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
|
||||
|
||||
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse();
|
||||
Vector xiprime3 = T3.Adjoint(screw::xi);
|
||||
Vector xiprime3 = T3.adjoint(screw::xi);
|
||||
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
|
||||
}
|
||||
|
||||
|
|
@ -259,7 +175,7 @@ TEST(Pose3, Adjoint_compose_full)
|
|||
const Pose3& T1 = T;
|
||||
Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8);
|
||||
Pose3 expected = T1 * Pose3::Expmap(x) * T2;
|
||||
Vector y = T2.inverse().Adjoint(x);
|
||||
Vector y = T2.inverse().adjoint(x);
|
||||
Pose3 actual = T1 * T2 * Pose3::Expmap(y);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -178,6 +178,11 @@ namespace gtsam {
|
|||
/** check equality */
|
||||
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
|
||||
|
||||
/** deep copy from another tree */
|
||||
void cloneTo(shared_ptr& newTree) const {
|
||||
root_->cloneToBayesTree(*newTree);
|
||||
}
|
||||
|
||||
/**
|
||||
* Find parent clique of a conditional. It will look at all parents and
|
||||
* return the one with the lowest index in the ordering.
|
||||
|
|
|
|||
|
|
@ -77,6 +77,20 @@ namespace gtsam {
|
|||
*/
|
||||
static derived_ptr Create(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) { return boost::make_shared<DerivedType>(result); }
|
||||
|
||||
void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const {
|
||||
sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_));
|
||||
sharedClique newClique = newTree.addClique(newConditional, parent_clique);
|
||||
if (cachedFactor_)
|
||||
newClique->cachedFactor_ = cachedFactor_->clone();
|
||||
else newClique->cachedFactor_ = typename FactorType::shared_ptr();
|
||||
if (!parent_clique) {
|
||||
newTree.root_ = newClique;
|
||||
}
|
||||
BOOST_FOREACH(const shared_ptr& childClique, children_) {
|
||||
childClique->cloneToBayesTree(newTree, newClique);
|
||||
}
|
||||
}
|
||||
|
||||
/** print this node */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
|
|
|
|||
|
|
@ -174,6 +174,13 @@ public:
|
|||
/** Access the container through the permutation (const version) */
|
||||
const value_type& operator[](size_t index) const { return container_[permutation_[index]]; }
|
||||
|
||||
/** Assignment operator for cloning in ISAM2 */
|
||||
Permuted<CONTAINER> operator=(const Permuted<CONTAINER>& other) {
|
||||
permutation_ = other.permutation_;
|
||||
container_ = other.container_;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Permute this view by applying a permutation to the underlying permutation */
|
||||
void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }
|
||||
|
||||
|
|
|
|||
|
|
@ -88,6 +88,9 @@ namespace gtsam {
|
|||
/** Return the dimension of the variable pointed to by the given key iterator */
|
||||
virtual size_t getDim(const_iterator variable) const = 0;
|
||||
|
||||
/** Clone a factor (make a deep copy) */
|
||||
virtual GaussianFactor::shared_ptr clone() const = 0;
|
||||
|
||||
/**
|
||||
* Permutes the GaussianFactor, but for efficiency requires the permutation
|
||||
* to already be inverted. This acts just as a change-of-name for each
|
||||
|
|
|
|||
|
|
@ -191,6 +191,12 @@ namespace gtsam {
|
|||
/** Destructor */
|
||||
virtual ~HessianFactor() {}
|
||||
|
||||
/** Clone this JacobianFactor */
|
||||
virtual GaussianFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<GaussianFactor>(
|
||||
shared_ptr(new HessianFactor(*this)));
|
||||
}
|
||||
|
||||
/** Print the factor for debugging and testing (implementing Testable) */
|
||||
virtual void print(const std::string& s = "") const;
|
||||
|
||||
|
|
|
|||
|
|
@ -136,6 +136,12 @@ namespace gtsam {
|
|||
|
||||
virtual ~JacobianFactor() {}
|
||||
|
||||
/** Clone this JacobianFactor */
|
||||
virtual GaussianFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<GaussianFactor>(
|
||||
shared_ptr(new JacobianFactor(*this)));
|
||||
}
|
||||
|
||||
// Implementing Testable interface
|
||||
virtual void print(const std::string& s = "") const;
|
||||
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
|
||||
|
|
|
|||
|
|
@ -27,12 +27,18 @@ namespace gtsam {
|
|||
*/
|
||||
template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
|
||||
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
|
||||
typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
|
||||
public:
|
||||
/** Create an empty ISAM2 instance */
|
||||
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
|
||||
|
||||
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
|
||||
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
|
||||
|
||||
void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
|
||||
boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);
|
||||
Base::cloneTo(isam2);
|
||||
}
|
||||
};
|
||||
|
||||
// optimize the BayesTree, starting from the root
|
||||
|
|
|
|||
|
|
@ -214,6 +214,27 @@ public:
|
|||
typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique
|
||||
typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class
|
||||
|
||||
void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const {
|
||||
boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2);
|
||||
Base::cloneTo(bayesTree);
|
||||
newISAM2->theta_ = theta_;
|
||||
newISAM2->variableIndex_ = variableIndex_;
|
||||
newISAM2->deltaUnpermuted_ = deltaUnpermuted_;
|
||||
newISAM2->delta_ = delta_;
|
||||
newISAM2->nonlinearFactors_ = nonlinearFactors_;
|
||||
newISAM2->ordering_ = ordering_;
|
||||
newISAM2->params_ = params_;
|
||||
#ifndef NDEBUG
|
||||
newISAM2->lastRelinVariables_ = lastRelinVariables_;
|
||||
#endif
|
||||
newISAM2->lastAffectedVariableCount = lastAffectedVariableCount;
|
||||
newISAM2->lastAffectedFactorCount = lastAffectedFactorCount;
|
||||
newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount;
|
||||
newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount;
|
||||
newISAM2->lastBacksubVariableCount = lastBacksubVariableCount;
|
||||
newISAM2->lastNnzTop = lastNnzTop;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add new factors, updating the solution and relinearizing as needed.
|
||||
*
|
||||
|
|
@ -279,6 +300,8 @@ public:
|
|||
size_t lastBacksubVariableCount;
|
||||
size_t lastNnzTop;
|
||||
|
||||
ISAM2Params params() const { return params_; }
|
||||
|
||||
//@}
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -15,24 +15,12 @@ headers += simulated2DConstraints.h
|
|||
sources += simulated2D.cpp smallExample.cpp
|
||||
check_PROGRAMS += tests/testSimulated2D
|
||||
|
||||
# MATLAB Wrap headers for simulated2D
|
||||
headers += Simulated2DMeasurement.h
|
||||
headers += Simulated2DOdometry.h
|
||||
headers += Simulated2DPointPrior.h
|
||||
headers += Simulated2DPosePrior.h
|
||||
headers += Simulated2DValues.h
|
||||
|
||||
# simulated2DOriented example
|
||||
sources += simulated2DOriented.cpp
|
||||
check_PROGRAMS += tests/testSimulated2DOriented
|
||||
|
||||
# MATLAB Wrap headers for simulated2DOriented
|
||||
headers += Simulated2DOrientedOdometry.h
|
||||
headers += Simulated2DOrientedPosePrior.h
|
||||
headers += Simulated2DOrientedValues.h
|
||||
|
||||
# simulated3D example
|
||||
sources += Simulated3D.cpp
|
||||
sources += simulated3D.cpp
|
||||
check_PROGRAMS += tests/testSimulated3D
|
||||
|
||||
# Generic SLAM headers
|
||||
|
|
@ -50,12 +38,6 @@ check_PROGRAMS += tests/testPose2SLAM
|
|||
sources += planarSLAM.cpp
|
||||
check_PROGRAMS += tests/testPlanarSLAM
|
||||
|
||||
# MATLAB Wrap headers for planarSLAM
|
||||
headers += Landmark2.h
|
||||
headers += PlanarSLAMGraph.h
|
||||
headers += PlanarSLAMValues.h
|
||||
headers += PlanarSLAMOdometry.h
|
||||
|
||||
# 3D Pose constraints
|
||||
sources += pose3SLAM.cpp
|
||||
check_PROGRAMS += tests/testPose3SLAM
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 Simulated2DMeasurement.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef simulated2D::Measurement Simulated2DMeasurement;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 Simulated2DOdometry.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef simulated2D::Odometry Simulated2DOdometry;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 Simulated2DOrientedOdometry.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Kai Ni
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/simulated2DOriented.h>
|
||||
#include <gtsam/slam/Simulated2DOrientedValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef simulated2DOriented::Odometry Simulated2DOrientedOdometry;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -1,29 +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 Simulated2DOrientedPosePrior.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Kai Ni
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/simulated2DOriented.h>
|
||||
#include <gtsam/slam/Simulated2DOrientedValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Create a prior on a pose Point2 with key 'x1' etc... */
|
||||
typedef simulated2DOriented::GenericPosePrior<Simulated2DOrientedValues, simulated2DOriented::PoseKey> Simulated2DOrientedPosePrior;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -1,59 +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 Simulated2DValues.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/simulated2DOriented.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Simulated2DOrientedValues: public simulated2DOriented::Values {
|
||||
public:
|
||||
typedef boost::shared_ptr<Point2> sharedPoint;
|
||||
typedef boost::shared_ptr<Pose2> sharedPose;
|
||||
|
||||
Simulated2DOrientedValues() {
|
||||
}
|
||||
|
||||
void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) {
|
||||
insert(i, p);
|
||||
}
|
||||
|
||||
void insertPoint(const simulated2DOriented::PointKey& j, const Point2& p) {
|
||||
insert(j, p);
|
||||
}
|
||||
|
||||
int nrPoses() const {
|
||||
return this->first_.size();
|
||||
}
|
||||
|
||||
int nrPoints() const {
|
||||
return this->second_.size();
|
||||
}
|
||||
|
||||
sharedPose pose(const simulated2DOriented::PoseKey& i) {
|
||||
return sharedPose(new Pose2((*this)[i]));
|
||||
}
|
||||
|
||||
sharedPoint point(const simulated2DOriented::PointKey& j) {
|
||||
return sharedPoint(new Point2((*this)[j]));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -1,29 +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 Simulated2DPointPrior.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Create a prior on a landmark Point2 with key 'l1' etc... */
|
||||
typedef simulated2D::GenericPrior<Simulated2DValues, simulated2D::PointKey> Simulated2DPointPrior;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -1,29 +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 Simulated2DPosePrior.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Create a prior on a pose Point2 with key 'x1' etc... */
|
||||
typedef simulated2D::GenericPrior<Simulated2DValues, simulated2D::PoseKey> Simulated2DPosePrior;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -1,58 +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 Simulated2DValues.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Simulated2DValues: public simulated2D::Values {
|
||||
public:
|
||||
typedef boost::shared_ptr<Point2> sharedPoint;
|
||||
|
||||
Simulated2DValues() {
|
||||
}
|
||||
|
||||
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
|
||||
insert(i, p);
|
||||
}
|
||||
|
||||
void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
|
||||
insert(j, p);
|
||||
}
|
||||
|
||||
int nrPoses() const {
|
||||
return this->first_.size();
|
||||
}
|
||||
|
||||
int nrPoints() const {
|
||||
return this->second_.size();
|
||||
}
|
||||
|
||||
sharedPoint pose(const simulated2D::PoseKey& i) {
|
||||
return sharedPoint(new Point2((*this)[i]));
|
||||
}
|
||||
|
||||
sharedPoint point(const simulated2D::PointKey& j) {
|
||||
return sharedPoint(new Point2((*this)[j]));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -15,7 +15,7 @@
|
|||
* @author Alex Cunningham
|
||||
**/
|
||||
|
||||
#include <gtsam/slam/Simulated3D.h>
|
||||
#include <gtsam/slam/simulated3D.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/nonlinear/TupleValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
// \namespace
|
||||
|
||||
|
|
@ -72,13 +73,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// Return pose i
|
||||
sharedPoint pose(const simulated2D::PoseKey& i) {
|
||||
return sharedPoint(new Point2((*this)[i]));
|
||||
Point2 pose(const simulated2D::PoseKey& i) const {
|
||||
return (*this)[i];
|
||||
}
|
||||
|
||||
/// Return point j
|
||||
sharedPoint point(const simulated2D::PointKey& j) {
|
||||
return sharedPoint(new Point2((*this)[j]));
|
||||
Point2 point(const simulated2D::PointKey& j) const {
|
||||
return (*this)[j];
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -232,5 +233,12 @@ namespace gtsam {
|
|||
typedef GenericOdometry<Values, PoseKey> Odometry;
|
||||
typedef GenericMeasurement<Values, PoseKey, PointKey> Measurement;
|
||||
|
||||
// Specialization of a graph for this example domain
|
||||
// TODO: add functions to add factor types
|
||||
class Graph : public NonlinearFactorGraph<Values> {
|
||||
public:
|
||||
Graph() {}
|
||||
};
|
||||
|
||||
} // namespace simulated2D
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/TupleValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
// \namespace
|
||||
|
||||
|
|
@ -33,7 +34,27 @@ namespace gtsam {
|
|||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
typedef Values<PoseKey> PoseValues;
|
||||
typedef Values<PointKey> PointValues;
|
||||
typedef TupleValues2<PoseValues, PointValues> Values;
|
||||
|
||||
/// Specialized Values structure with syntactic sugar for
|
||||
/// compatibility with matlab
|
||||
class Values: public TupleValues2<PoseValues, PointValues> {
|
||||
public:
|
||||
Values() {}
|
||||
|
||||
void insertPose(const PoseKey& i, const Pose2& p) {
|
||||
insert(i, p);
|
||||
}
|
||||
|
||||
void insertPoint(const PointKey& j, const Point2& p) {
|
||||
insert(j, p);
|
||||
}
|
||||
|
||||
int nrPoses() const { return this->first_.size(); }
|
||||
int nrPoints() const { return this->second_.size(); }
|
||||
|
||||
Pose2 pose(const PoseKey& i) const { return (*this)[i]; }
|
||||
Point2 point(const PointKey& j) const { return (*this)[j]; }
|
||||
};
|
||||
|
||||
//TODO:: point prior is not implemented right now
|
||||
|
||||
|
|
@ -100,5 +121,12 @@ namespace gtsam {
|
|||
|
||||
typedef GenericOdometry<Values, PoseKey> Odometry;
|
||||
|
||||
/// Graph specialization for syntactic sugar use with matlab
|
||||
class Graph : public NonlinearFactorGraph<Values> {
|
||||
public:
|
||||
Graph() {}
|
||||
// TODO: add functions to add factors
|
||||
};
|
||||
|
||||
} // namespace simulated2DOriented
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,43 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Simulated3D.cpp
|
||||
* @brief measurement functions and derivatives for simulated 3D robot
|
||||
* @author Alex Cunningham
|
||||
**/
|
||||
|
||||
#include <gtsam/slam/simulated3D.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace simulated3D {
|
||||
|
||||
Point3 prior (const Point3& x, boost::optional<Matrix&> H) {
|
||||
if (H) *H = eye(3);
|
||||
return x;
|
||||
}
|
||||
|
||||
Point3 odo(const Point3& x1, const Point3& x2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
|
||||
if (H1) *H1 = -1 * eye(3);
|
||||
if (H2) *H2 = eye(3);
|
||||
return x2 - x1;
|
||||
}
|
||||
|
||||
Point3 mea(const Point3& x, const Point3& l,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
|
||||
if (H1) *H1 = -1 * eye(3);
|
||||
if (H2) *H2 = eye(3);
|
||||
return l - x;
|
||||
}
|
||||
|
||||
}} // namespace gtsam::simulated3D
|
||||
|
|
@ -0,0 +1,132 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Simulated3D.h
|
||||
* @brief measurement functions and derivatives for simulated 3D robot
|
||||
* @author Alex Cunningham
|
||||
**/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/nonlinear/TupleValues.h>
|
||||
|
||||
// \namespace
|
||||
|
||||
namespace gtsam {
|
||||
namespace simulated3D {
|
||||
|
||||
/**
|
||||
* This is a linear SLAM domain where both poses and landmarks are
|
||||
* 3D points, without rotation. The structure and use is based on
|
||||
* the simulated2D domain.
|
||||
*/
|
||||
|
||||
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
|
||||
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
|
||||
|
||||
typedef Values<PoseKey> PoseValues;
|
||||
typedef Values<PointKey> PointValues;
|
||||
typedef TupleValues2<PoseValues, PointValues> Values;
|
||||
|
||||
/**
|
||||
* Prior on a single pose
|
||||
*/
|
||||
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none);
|
||||
|
||||
/**
|
||||
* odometry between two poses
|
||||
*/
|
||||
Point3 odo(const Point3& x1, const Point3& x2,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none);
|
||||
|
||||
/**
|
||||
* measurement between landmark and pose
|
||||
*/
|
||||
Point3 mea(const Point3& x, const Point3& l,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none);
|
||||
|
||||
/**
|
||||
* A prior factor on a single linear robot pose
|
||||
*/
|
||||
struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
|
||||
|
||||
Point3 z_; ///< 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 model is the measurement model for the factor (Dimension: 3)
|
||||
* @param j is the key for the pose
|
||||
*/
|
||||
PointPrior3D(const Point3& z,
|
||||
const SharedNoiseModel& model, const PoseKey& j) :
|
||||
NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
|
||||
}
|
||||
|
||||
/**
|
||||
* Evaluates the error at a given value of x,
|
||||
* with optional derivatives.
|
||||
* @param x is the current value of the variable
|
||||
* @param H is an optional Jacobian matrix (Dimension: 3x3)
|
||||
* @return Vector error between prior value and x (Dimension: 3)
|
||||
*/
|
||||
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
|
||||
boost::none) {
|
||||
return (prior(x, H) - z_).vector();
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Models a linear 3D measurement between 3D points
|
||||
*/
|
||||
struct Simulated3DMeasurement: public NonlinearFactor2<Values,
|
||||
PoseKey, PointKey> {
|
||||
|
||||
Point3 z_; ///< 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 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
|
||||
*/
|
||||
Simulated3DMeasurement(const Point3& z,
|
||||
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
|
||||
NonlinearFactor2<Values, PoseKey, PointKey> (
|
||||
model, j1, j2), z_(z) {
|
||||
}
|
||||
|
||||
/**
|
||||
* Error function with optional derivatives
|
||||
* @param x1 a robot pose value
|
||||
* @param x2 a landmark point value
|
||||
* @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3)
|
||||
* @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<Matrix&> H2 = boost::none) {
|
||||
return (mea(x1, x2, H1, H2) - z_).vector();
|
||||
}
|
||||
};
|
||||
|
||||
}} // namespace simulated3D
|
||||
|
|
@ -21,7 +21,7 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/slam/Simulated3D.h>
|
||||
#include <gtsam/slam/simulated3D.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +1,12 @@
|
|||
../configure --prefix=$HOME --with-toolbox=$HOME/toolbox --enable-build-toolbox CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --disable-fast-install
|
||||
../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 \
|
||||
CFLAGS="-fno-inline -g -Wall" \
|
||||
CXXFLAGS="-fno-inline -g -Wall" \
|
||||
LDFLAGS="-fno-inline -g -Wall" \
|
||||
--disable-static \
|
||||
--disable-fast-install
|
||||
|
|
|
|||
|
|
@ -0,0 +1,10 @@
|
|||
% Test runner script - runs each test
|
||||
|
||||
display 'Starting: testJacobianFactor'
|
||||
testJacobianFactor
|
||||
|
||||
display 'Starting: testKalmanFilter'
|
||||
testKalmanFilter
|
||||
|
||||
% end of tests
|
||||
display 'Tests complete!'
|
||||
|
|
@ -292,6 +292,110 @@ TEST(ISAM2, slamlike_solution)
|
|||
EXPECT(isam_check(fullgraph, fullinit, isam));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE(ISAM2, clone) {
|
||||
|
||||
// Pose and landmark key types from planarSLAM
|
||||
typedef planarSLAM::PoseKey PoseKey;
|
||||
typedef planarSLAM::PointKey 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<planarSLAM::Values> isam(ISAM2Params(0.001, 0.0, 0, false, true));
|
||||
planarSLAM::Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
// i keeps track of the time step
|
||||
size_t i = 0;
|
||||
|
||||
// Add a prior at time 0 and update isam
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::Values init;
|
||||
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
|
||||
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
EXPECT(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Add odometry from time 0 to time 5
|
||||
for( ; i<5; ++i) {
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::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));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
// Add odometry from time 5 to 6 and landmark measurement at time 5
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
|
||||
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::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)));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
|
||||
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
|
||||
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
++ i;
|
||||
}
|
||||
|
||||
// Add odometry from time 6 to time 10
|
||||
for( ; i<10; ++i) {
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::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));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
// Add odometry from time 10 to 11 and landmark measurement at time 10
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
|
||||
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::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));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
++ i;
|
||||
}
|
||||
|
||||
// CLONING...
|
||||
boost::shared_ptr<GaussianISAM2<planarSLAM::Values> > isam2
|
||||
= boost::shared_ptr<GaussianISAM2<planarSLAM::Values> >(new GaussianISAM2<planarSLAM::Values>());
|
||||
isam.cloneTo(isam2);
|
||||
|
||||
CHECK(assert_equal(isam, *isam2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -22,34 +22,43 @@
|
|||
#include "Argument.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Argument::matlab_unwrap(ofstream& ofs,
|
||||
const string& matlabName)
|
||||
{
|
||||
void Argument::matlab_unwrap(ofstream& ofs, const string& matlabName) const {
|
||||
ofs << " ";
|
||||
|
||||
string cppType = qualifiedType("::");
|
||||
string matlabType = qualifiedType();
|
||||
|
||||
if (is_ptr)
|
||||
// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
|
||||
ofs << "shared_ptr<" << type << "> " << name << " = unwrap_shared_ptr< ";
|
||||
ofs << "shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< ";
|
||||
else if (is_ref)
|
||||
// A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
|
||||
ofs << type << "& " << name << " = *unwrap_shared_ptr< ";
|
||||
ofs << cppType << "& " << name << " = *unwrap_shared_ptr< ";
|
||||
else
|
||||
// Not a pointer or a reference: emit an "unwrap" call
|
||||
// unwrap is specified in matlab.h as a series of template specializations
|
||||
// that know how to unpack the expected MATLAB object
|
||||
// example: double tol = unwrap< double >(in[2]);
|
||||
// example: Vector v = unwrap< Vector >(in[1]);
|
||||
ofs << type << " " << name << " = unwrap< ";
|
||||
ofs << cppType << " " << name << " = unwrap< ";
|
||||
|
||||
ofs << type << " >(" << matlabName;
|
||||
if (is_ptr || is_ref) ofs << ", \"" << type << "\"";
|
||||
ofs << cppType << " >(" << matlabName;
|
||||
if (is_ptr || is_ref) ofs << ", \"" << matlabType << "\"";
|
||||
ofs << ");" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ArgumentList::types() {
|
||||
string Argument::qualifiedType(const string& delim) const {
|
||||
string result;
|
||||
BOOST_FOREACH(const string& ns, namespaces) result += ns + delim;
|
||||
return result + type;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ArgumentList::types() const {
|
||||
string str;
|
||||
bool first=true;
|
||||
BOOST_FOREACH(Argument arg, *this) {
|
||||
|
|
@ -59,7 +68,7 @@ string ArgumentList::types() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ArgumentList::signature() {
|
||||
string ArgumentList::signature() const {
|
||||
string str;
|
||||
BOOST_FOREACH(Argument arg, *this)
|
||||
str += arg.type[0];
|
||||
|
|
@ -67,7 +76,7 @@ string ArgumentList::signature() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ArgumentList::names() {
|
||||
string ArgumentList::names() const {
|
||||
string str;
|
||||
bool first=true;
|
||||
BOOST_FOREACH(Argument arg, *this) {
|
||||
|
|
@ -77,7 +86,7 @@ string ArgumentList::names() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ArgumentList::matlab_unwrap(ofstream& ofs, int start) {
|
||||
void ArgumentList::matlab_unwrap(ofstream& ofs, int start) const {
|
||||
int index = start;
|
||||
BOOST_FOREACH(Argument arg, *this) {
|
||||
stringstream buf;
|
||||
|
|
|
|||
|
|
@ -18,27 +18,32 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/// Argument class
|
||||
struct Argument {
|
||||
bool is_const, is_ref, is_ptr;
|
||||
std::string type;
|
||||
std::string name;
|
||||
std::vector<std::string> namespaces;
|
||||
Argument() :
|
||||
is_const(false), is_ref(false), is_ptr(false) {
|
||||
}
|
||||
|
||||
std::string qualifiedType(const std::string& delim = "") const; // adds namespaces to type
|
||||
|
||||
/// MATLAB code generation, MATLAB to C++
|
||||
void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName);
|
||||
void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName) const;
|
||||
};
|
||||
|
||||
/// Argument list
|
||||
struct ArgumentList: public std::list<Argument> {
|
||||
std::list<Argument> args;
|
||||
std::string types();
|
||||
std::string signature();
|
||||
std::string names();
|
||||
struct ArgumentList: public std::vector<Argument> {
|
||||
std::vector<Argument> args; // why does it contain this?
|
||||
std::string types() const;
|
||||
std::string signature() const;
|
||||
std::string names() const;
|
||||
|
||||
// MATLAB code generation:
|
||||
|
||||
|
|
@ -47,6 +52,8 @@ struct ArgumentList: public std::list<Argument> {
|
|||
* @param ofs output stream
|
||||
* @param start initial index for input array, set to 1 for method
|
||||
*/
|
||||
void matlab_unwrap(std::ofstream& ofs, int start = 0); // MATLAB to C++
|
||||
void matlab_unwrap(std::ofstream& ofs, int start = 0) const; // MATLAB to C++
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
||||
|
|
|
|||
100
wrap/Class.cpp
100
wrap/Class.cpp
|
|
@ -14,6 +14,7 @@
|
|||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
|
|
@ -23,24 +24,28 @@
|
|||
#include "utilities.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::matlab_proxy(const string& classFile) {
|
||||
void Class::matlab_proxy(const string& classFile) const {
|
||||
// open destination classFile
|
||||
ofstream ofs(classFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(classFile);
|
||||
if(verbose_) cerr << "generating " << classFile << endl;
|
||||
|
||||
// get the name of actual matlab object
|
||||
string matlabName = qualifiedName();
|
||||
|
||||
// emit class proxy code
|
||||
ofs << "classdef " << name << endl;
|
||||
ofs << "classdef " << matlabName << endl;
|
||||
ofs << " properties" << endl;
|
||||
ofs << " self = 0" << endl;
|
||||
ofs << " end" << endl;
|
||||
ofs << " methods" << endl;
|
||||
ofs << " function obj = " << name << "(varargin)" << endl;
|
||||
ofs << " function obj = " << matlabName << "(varargin)" << endl;
|
||||
BOOST_FOREACH(Constructor c, constructors)
|
||||
c.matlab_proxy_fragment(ofs,name);
|
||||
ofs << " if nargin ~= 13 && obj.self == 0, error('" << name << " constructor failed'); end" << endl;
|
||||
c.matlab_proxy_fragment(ofs,matlabName);
|
||||
ofs << " if nargin ~= 13 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl;
|
||||
ofs << " end" << endl;
|
||||
ofs << " function display(obj), obj.print(''); end" << endl;
|
||||
ofs << " function disp(obj), obj.display; end" << endl;
|
||||
|
|
@ -52,33 +57,98 @@ void Class::matlab_proxy(const string& classFile) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::matlab_constructors(const string& toolboxPath,const string& nameSpace) {
|
||||
void Class::matlab_constructors(const string& toolboxPath, const vector<string>& using_namespaces) const {
|
||||
BOOST_FOREACH(Constructor c, constructors) {
|
||||
c.matlab_mfile (toolboxPath, name);
|
||||
c.matlab_wrapper(toolboxPath, name, nameSpace);
|
||||
c.matlab_mfile (toolboxPath, qualifiedName());
|
||||
c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::matlab_methods(const string& classPath, const string& nameSpace) {
|
||||
void Class::matlab_methods(const string& classPath, const vector<string>& using_namespaces) const {
|
||||
string matlabName = qualifiedName(), cppName = qualifiedName("::");
|
||||
BOOST_FOREACH(Method m, methods) {
|
||||
m.matlab_mfile (classPath);
|
||||
m.matlab_wrapper(classPath, name, nameSpace);
|
||||
m.matlab_wrapper(classPath, name, cppName, matlabName, using_namespaces, includes);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::matlab_static_methods(const string& toolboxPath, const vector<string>& using_namespaces) const {
|
||||
string matlabName = qualifiedName(), cppName = qualifiedName("::");
|
||||
BOOST_FOREACH(const StaticMethod& m, static_methods) {
|
||||
m.matlab_mfile (toolboxPath, qualifiedName());
|
||||
m.matlab_wrapper(toolboxPath, name, matlabName, cppName, using_namespaces, includes);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::matlab_make_fragment(ofstream& ofs,
|
||||
const string& toolboxPath,
|
||||
const string& mexFlags)
|
||||
{
|
||||
const string& mexFlags) const {
|
||||
string mex = "mex " + mexFlags + " ";
|
||||
string matlabClassName = qualifiedName();
|
||||
BOOST_FOREACH(Constructor c, constructors)
|
||||
ofs << mex << c.matlab_wrapper_name(name) << ".cpp" << endl;
|
||||
ofs << endl << "cd @" << name << endl;
|
||||
ofs << mex << c.matlab_wrapper_name(matlabClassName) << ".cpp" << endl;
|
||||
BOOST_FOREACH(StaticMethod sm, static_methods)
|
||||
ofs << mex << matlabClassName + "_" + sm.name << ".cpp" << endl;
|
||||
ofs << endl << "cd @" << matlabClassName << endl;
|
||||
BOOST_FOREACH(Method m, methods)
|
||||
ofs << mex << m.name_ << ".cpp" << endl;
|
||||
ofs << mex << m.name << ".cpp" << endl;
|
||||
ofs << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::makefile_fragment(ofstream& ofs) const {
|
||||
// new_Point2_.$(MEXENDING): new_Point2_.cpp
|
||||
// $(MEX) $(mex_flags) new_Point2_.cpp
|
||||
// new_Point2_dd.$(MEXENDING): new_Point2_dd.cpp
|
||||
// $(MEX) $(mex_flags) new_Point2_dd.cpp
|
||||
// @Point2/x.$(MEXENDING): @Point2/x.cpp
|
||||
// $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x
|
||||
// @Point2/y.$(MEXENDING): @Point2/y.cpp
|
||||
// $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y
|
||||
// @Point2/dim.$(MEXENDING): @Point2/dim.cpp
|
||||
// $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim
|
||||
//
|
||||
// Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING)
|
||||
|
||||
string matlabName = qualifiedName();
|
||||
|
||||
// collect names
|
||||
vector<string> file_names;
|
||||
BOOST_FOREACH(Constructor c, constructors) {
|
||||
string file_base = c.matlab_wrapper_name(matlabName);
|
||||
file_names.push_back(file_base);
|
||||
}
|
||||
BOOST_FOREACH(StaticMethod c, static_methods) {
|
||||
string file_base = matlabName + "_" + c.name;
|
||||
file_names.push_back(file_base);
|
||||
}
|
||||
BOOST_FOREACH(Method c, methods) {
|
||||
string file_base = "@" + matlabName + "/" + c.name;
|
||||
file_names.push_back(file_base);
|
||||
}
|
||||
|
||||
BOOST_FOREACH(const string& file_base, file_names) {
|
||||
ofs << file_base << ".$(MEXENDING): " << file_base << ".cpp" << endl;
|
||||
ofs << "\t$(MEX) $(mex_flags) " << file_base << ".cpp -output " << file_base << endl;
|
||||
}
|
||||
|
||||
// class target
|
||||
ofs << "\n" << matlabName << ": ";
|
||||
BOOST_FOREACH(const string& file_base, file_names) {
|
||||
ofs << file_base << ".$(MEXENDING) ";
|
||||
}
|
||||
ofs << "\n" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string Class::qualifiedName(const string& delim) const {
|
||||
string result;
|
||||
BOOST_FOREACH(const string& ns, namespaces)
|
||||
result += ns + delim;
|
||||
return result + name;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
29
wrap/Class.h
29
wrap/Class.h
|
|
@ -18,10 +18,12 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <list>
|
||||
|
||||
#include "Constructor.h"
|
||||
#include "Method.h"
|
||||
#include "StaticMethod.h"
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/// Class has name, constructors, methods
|
||||
struct Class {
|
||||
|
|
@ -29,19 +31,28 @@ struct Class {
|
|||
Class(bool verbose=true) : verbose_(verbose) {}
|
||||
|
||||
// Then the instance variables are set directly by the Module constructor
|
||||
std::string name; ///< Class name
|
||||
std::list<Constructor> constructors; ///< Class constructors
|
||||
std::list<Method> methods; ///< Class methods
|
||||
bool verbose_; ///< verbose flag
|
||||
std::string name; ///< Class name
|
||||
std::vector<Constructor> constructors; ///< Class constructors
|
||||
std::vector<Method> methods; ///< Class methods
|
||||
std::vector<StaticMethod> static_methods; ///< Static methods
|
||||
std::vector<std::string> namespaces; ///< Stack of namespaces
|
||||
std::vector<std::string> includes; ///< header include overrides
|
||||
bool verbose_; ///< verbose flag
|
||||
|
||||
// And finally MATLAB code is emitted, methods below called by Module::matlab_code
|
||||
void matlab_proxy(const std::string& classFile); ///< emit proxy class
|
||||
void matlab_proxy(const std::string& classFile) const; ///< emit proxy class
|
||||
void matlab_constructors(const std::string& toolboxPath,
|
||||
const std::string& nameSpace); ///< emit constructor wrappers
|
||||
const std::vector<std::string>& using_namespaces) const; ///< emit constructor wrappers
|
||||
void matlab_methods(const std::string& classPath,
|
||||
const std::string& nameSpace); ///< emit method wrappers
|
||||
const std::vector<std::string>& using_namespaces) const; ///< emit method wrappers
|
||||
void matlab_static_methods(const std::string& classPath,
|
||||
const std::vector<std::string>& using_namespaces) const; ///< emit static method wrappers
|
||||
void matlab_make_fragment(std::ofstream& ofs,
|
||||
const std::string& toolboxPath,
|
||||
const std::string& mexFlags); ///< emit make fragment for global make script
|
||||
const std::string& mexFlags) const; ///< emit make fragment for global make script
|
||||
void makefile_fragment(std::ofstream& ofs) const; ///< emit makefile fragment
|
||||
std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
||||
|
|
|
|||
|
|
@ -23,15 +23,16 @@
|
|||
#include "Constructor.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
string Constructor::matlab_wrapper_name(const string& className) {
|
||||
string Constructor::matlab_wrapper_name(const string& className) const {
|
||||
string str = "new_" + className + "_" + args.signature();
|
||||
return str;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) {
|
||||
void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) const {
|
||||
ofs << " if nargin == " << args.size() << ", obj.self = "
|
||||
<< matlab_wrapper_name(className) << "(";
|
||||
bool first = true;
|
||||
|
|
@ -44,22 +45,22 @@ void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Constructor::matlab_mfile(const string& toolboxPath, const string& className) {
|
||||
void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName) const {
|
||||
|
||||
string name = matlab_wrapper_name(className);
|
||||
string matlabName = matlab_wrapper_name(qualifiedMatlabName);
|
||||
|
||||
// open destination m-file
|
||||
string wrapperFile = toolboxPath + "/" + name + ".m";
|
||||
string wrapperFile = toolboxPath + "/" + matlabName + ".m";
|
||||
ofstream ofs(wrapperFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(wrapperFile);
|
||||
if(verbose_) cerr << "generating " << wrapperFile << endl;
|
||||
|
||||
// generate code
|
||||
emit_header_comment(ofs, "%");
|
||||
ofs << "function result = " << name << "(obj";
|
||||
generateHeaderComment(ofs, "%");
|
||||
ofs << "function result = " << matlabName << "(obj";
|
||||
if (args.size()) ofs << "," << args.names();
|
||||
ofs << ")" << endl;
|
||||
ofs << " error('need to compile " << name << ".cpp');" << endl;
|
||||
ofs << " error('need to compile " << matlabName << ".cpp');" << endl;
|
||||
ofs << "end" << endl;
|
||||
|
||||
// close file
|
||||
|
|
@ -68,29 +69,28 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& classNam
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Constructor::matlab_wrapper(const string& toolboxPath,
|
||||
const string& className,
|
||||
const string& nameSpace)
|
||||
{
|
||||
|
||||
string name = matlab_wrapper_name(className);
|
||||
const string& cppClassName,
|
||||
const string& matlabClassName,
|
||||
const vector<string>& using_namespaces, const vector<string>& includes) const {
|
||||
string matlabName = matlab_wrapper_name(matlabClassName);
|
||||
|
||||
// open destination wrapperFile
|
||||
string wrapperFile = toolboxPath + "/" + name + ".cpp";
|
||||
string wrapperFile = toolboxPath + "/" + matlabName + ".cpp";
|
||||
ofstream ofs(wrapperFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(wrapperFile);
|
||||
if(verbose_) cerr << "generating " << wrapperFile << endl;
|
||||
|
||||
// generate code
|
||||
emit_header_comment(ofs, "//");
|
||||
ofs << "#include <wrap/matlab.h>" << endl;
|
||||
ofs << "#include <" << className << ".h>" << endl;
|
||||
if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl;
|
||||
generateHeaderComment(ofs, "//");
|
||||
generateIncludes(ofs, name, includes);
|
||||
generateUsingNamespace(ofs, using_namespaces);
|
||||
|
||||
ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
|
||||
ofs << "{" << endl;
|
||||
ofs << " checkArguments(\"" << name << "\",nargout,nargin," << args.size() << ");" << endl;
|
||||
ofs << " checkArguments(\"" << matlabName << "\",nargout,nargin," << args.size() << ");" << endl;
|
||||
args.matlab_unwrap(ofs); // unwrap arguments
|
||||
ofs << " " << className << "* self = new " << className << "(" << args.names() << ");" << endl;
|
||||
ofs << " out[0] = wrap_constructed(self,\"" << className << "\");" << endl;
|
||||
ofs << " " << cppClassName << "* self = new " << cppClassName << "(" << args.names() << ");" << endl; // need qualified name, delim: "::"
|
||||
ofs << " out[0] = wrap_constructed(self,\"" << matlabClassName << "\");" << endl; // need matlab qualified name
|
||||
ofs << "}" << endl;
|
||||
|
||||
// close file
|
||||
|
|
|
|||
|
|
@ -18,10 +18,12 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
#include "Argument.h"
|
||||
|
||||
namespace wrap {
|
||||
|
||||
// Constructor class
|
||||
struct Constructor {
|
||||
|
||||
|
|
@ -32,6 +34,7 @@ struct Constructor {
|
|||
|
||||
// Then the instance variables are set directly by the Module constructor
|
||||
ArgumentList args;
|
||||
std::string name;
|
||||
bool verbose_;
|
||||
|
||||
// MATLAB code generation
|
||||
|
|
@ -39,17 +42,22 @@ struct Constructor {
|
|||
// classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m
|
||||
|
||||
/// wrapper name
|
||||
std::string matlab_wrapper_name(const std::string& className);
|
||||
std::string matlab_wrapper_name(const std::string& className) const;
|
||||
|
||||
/// proxy class fragment
|
||||
void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className);
|
||||
void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className) const;
|
||||
|
||||
/// m-file
|
||||
void matlab_mfile(const std::string& toolboxPath,
|
||||
const std::string& className);
|
||||
const std::string& qualifiedMatlabName) const;
|
||||
|
||||
/// wrapper
|
||||
/// cpp wrapper
|
||||
void matlab_wrapper(const std::string& toolboxPath,
|
||||
const std::string& className, const std::string& nameSpace);
|
||||
const std::string& cppClassName,
|
||||
const std::string& matlabClassName,
|
||||
const std::vector<std::string>& using_namespaces,
|
||||
const std::vector<std::string>& includes) const;
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
||||
|
|
|
|||
|
|
@ -10,14 +10,21 @@ AM_DEFAULT_SOURCE_EXT = .cpp
|
|||
headers =
|
||||
sources =
|
||||
check_PROGRAMS =
|
||||
noinst_PROGRAMS =
|
||||
wrap_PROGRAMS =
|
||||
wrapdir = $(pkgincludedir)/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 Constructor.cpp Method.cpp Class.cpp Module.cpp
|
||||
sources += utilities.cpp Argument.cpp ReturnValue.cpp Constructor.cpp Method.cpp StaticMethod.cpp Class.cpp Module.cpp
|
||||
check_PROGRAMS += tests/testSpirit tests/testWrap
|
||||
noinst_PROGRAMS = wrap
|
||||
if ENABLE_INSTALL_WRAP
|
||||
wrap_PROGRAMS += wrap
|
||||
else
|
||||
noinst_PROGRAMS += wrap
|
||||
endif
|
||||
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
# Create a libtool library that is not installed
|
||||
|
|
@ -25,7 +32,6 @@ noinst_PROGRAMS = wrap
|
|||
#----------------------------------------------------------------------------------------------------
|
||||
# Only install the header necessary for wrap interfaces to build with mex
|
||||
headers += matlab.h
|
||||
wrapdir = $(pkgincludedir)/wrap
|
||||
wrap_HEADERS = $(headers)
|
||||
noinst_LTLIBRARIES = libwrap.la
|
||||
libwrap_la_SOURCES = $(sources)
|
||||
|
|
@ -51,15 +57,74 @@ LDADD = libwrap.la ../CppUnitLite/libCppUnitLite.a
|
|||
interfacePath = $(top_srcdir)
|
||||
moduleName = gtsam
|
||||
toolboxpath = ../toolbox
|
||||
nameSpace = "gtsam"
|
||||
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"
|
||||
all:
|
||||
./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags}
|
||||
|
||||
# install the headers and matlab toolbox
|
||||
install-exec-hook: all
|
||||
install -d ${toolbox}/gtsam && \
|
||||
cp -rf ../toolbox/* ${toolbox}/gtsam
|
||||
# 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
|
||||
|
||||
all: generate_toolbox
|
||||
|
||||
generate_toolbox: $(top_srcdir)/gtsam.h
|
||||
./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${mexFlags}
|
||||
|
||||
source_mode = -m 644
|
||||
|
||||
wrap-install-matlab-toolbox: generate_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: 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:
|
||||
|
|
|
|||
|
|
@ -23,44 +23,24 @@
|
|||
#include "utilities.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// auxiliary function to wrap an argument into a shared_ptr template
|
||||
/* ************************************************************************* */
|
||||
string maybe_shared_ptr(bool add, const string& type) {
|
||||
string str = add? "shared_ptr<" : "";
|
||||
str += type;
|
||||
if (add) str += ">";
|
||||
return str;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string Method::return_type(bool add_ptr, pairing p) {
|
||||
if (p==pair && returns_pair_) {
|
||||
string str = "pair< " +
|
||||
maybe_shared_ptr(add_ptr && returns_ptr_, returns_) + ", " +
|
||||
maybe_shared_ptr(add_ptr && returns_ptr_, returns2_) + " >";
|
||||
return str;
|
||||
} else
|
||||
return maybe_shared_ptr(add_ptr && returns_ptr_, (p==arg2)? returns2_ : returns_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Method::matlab_mfile(const string& classPath) {
|
||||
void Method::matlab_mfile(const string& classPath) const {
|
||||
|
||||
// open destination m-file
|
||||
string wrapperFile = classPath + "/" + name_ + ".m";
|
||||
string wrapperFile = classPath + "/" + name + ".m";
|
||||
ofstream ofs(wrapperFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(wrapperFile);
|
||||
if(verbose_) cerr << "generating " << wrapperFile << endl;
|
||||
|
||||
// generate code
|
||||
string returnType = returns_pair_? "[first,second]" : "result";
|
||||
ofs << "function " << returnType << " = " << name_ << "(obj";
|
||||
if (args_.size()) ofs << "," << args_.names();
|
||||
string returnType = returnVal.matlab_returnType();
|
||||
ofs << "function " << returnType << " = " << name << "(obj";
|
||||
if (args.size()) ofs << "," << args.names();
|
||||
ofs << ")" << endl;
|
||||
ofs << "% usage: obj." << name_ << "(" << args_.names() << ")" << endl;
|
||||
ofs << " error('need to compile " << name_ << ".cpp');" << endl;
|
||||
ofs << "% usage: obj." << name << "(" << args.names() << ")" << endl;
|
||||
ofs << " error('need to compile " << name << ".cpp');" << endl;
|
||||
ofs << "end" << endl;
|
||||
|
||||
// close file
|
||||
|
|
@ -70,10 +50,11 @@ void Method::matlab_mfile(const string& classPath) {
|
|||
/* ************************************************************************* */
|
||||
void Method::matlab_wrapper(const string& classPath,
|
||||
const string& className,
|
||||
const string& nameSpace)
|
||||
{
|
||||
const string& cppClassName,
|
||||
const string& matlabClassName,
|
||||
const vector<string>& using_namespaces, const std::vector<std::string>& includes) const {
|
||||
// open destination wrapperFile
|
||||
string wrapperFile = classPath + "/" + name_ + ".cpp";
|
||||
string wrapperFile = classPath + "/" + name + ".cpp";
|
||||
ofstream ofs(wrapperFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(wrapperFile);
|
||||
if(verbose_) cerr << "generating " << wrapperFile << endl;
|
||||
|
|
@ -81,10 +62,9 @@ void Method::matlab_wrapper(const string& classPath,
|
|||
// generate code
|
||||
|
||||
// header
|
||||
emit_header_comment(ofs, "//");
|
||||
ofs << "#include <wrap/matlab.h>\n";
|
||||
ofs << "#include <" << className << ".h>\n";
|
||||
if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl;
|
||||
generateHeaderComment(ofs, "//");
|
||||
generateIncludes(ofs, className, includes);
|
||||
generateUsingNamespace(ofs, using_namespaces);
|
||||
|
||||
// call
|
||||
ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
|
||||
|
|
@ -94,39 +74,26 @@ void Method::matlab_wrapper(const string& classPath,
|
|||
// check arguments
|
||||
// extra argument obj -> nargin-1 is passed !
|
||||
// example: checkArguments("equals",nargout,nargin-1,2);
|
||||
ofs << " checkArguments(\"" << name_ << "\",nargout,nargin-1," << args_.size() << ");\n";
|
||||
ofs << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n";
|
||||
|
||||
// get class pointer
|
||||
// example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
|
||||
ofs << " shared_ptr<" << className << "> self = unwrap_shared_ptr< " << className
|
||||
<< " >(in[0],\"" << className << "\");" << endl;
|
||||
ofs << " shared_ptr<" << cppClassName << "> self = unwrap_shared_ptr< " << cppClassName
|
||||
<< " >(in[0],\"" << matlabClassName << "\");" << endl;
|
||||
|
||||
// unwrap arguments, see Argument.cpp
|
||||
args_.matlab_unwrap(ofs,1);
|
||||
args.matlab_unwrap(ofs,1);
|
||||
|
||||
// call method
|
||||
// example: bool result = self->return_field(t);
|
||||
ofs << " ";
|
||||
if (returns_!="void")
|
||||
ofs << return_type(true,pair) << " result = ";
|
||||
ofs << "self->" << name_ << "(" << args_.names() << ");\n";
|
||||
if (returnVal.type1!="void")
|
||||
ofs << returnVal.return_type(true,ReturnValue::pair) << " result = ";
|
||||
ofs << "self->" << name << "(" << args.names() << ");\n";
|
||||
|
||||
// wrap result
|
||||
// example: out[0]=wrap<bool>(result);
|
||||
if (returns_pair_) {
|
||||
if (returns_ptr_)
|
||||
ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns_ << "\");\n";
|
||||
else
|
||||
ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n";
|
||||
if (returns_ptr2_)
|
||||
ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2_ << "\");\n";
|
||||
else
|
||||
ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
|
||||
}
|
||||
else if (returns_ptr_)
|
||||
ofs << " out[0] = wrap_shared_ptr(result,\"" << returns_ << "\");\n";
|
||||
else if (returns_!="void")
|
||||
ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n";
|
||||
returnVal.wrap_result(ofs);
|
||||
|
||||
// finish
|
||||
ofs << "}\n";
|
||||
|
|
|
|||
|
|
@ -21,33 +21,35 @@
|
|||
#include <list>
|
||||
|
||||
#include "Argument.h"
|
||||
#include "ReturnValue.h"
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/// Method class
|
||||
struct Method {
|
||||
|
||||
/// Constructor creates empty object
|
||||
Method(bool verbose = true) :
|
||||
returns_ptr_(false), returns_ptr2_(false), returns_pair_(false), verbose_(
|
||||
verbose) {
|
||||
}
|
||||
verbose_(verbose) {}
|
||||
|
||||
// Then the instance variables are set directly by the Module constructor
|
||||
bool is_const_;
|
||||
ArgumentList args_;
|
||||
std::string returns_, returns2_, name_;
|
||||
bool returns_ptr_, returns_ptr2_, returns_pair_;
|
||||
bool verbose_;
|
||||
|
||||
enum pairing {
|
||||
arg1, arg2, pair
|
||||
};
|
||||
std::string return_type(bool add_ptr, pairing p);
|
||||
bool is_const_;
|
||||
std::string name;
|
||||
ArgumentList args;
|
||||
ReturnValue returnVal;
|
||||
|
||||
// MATLAB code generation
|
||||
// classPath is class directory, e.g., ../matlab/@Point2
|
||||
|
||||
void matlab_mfile(const std::string& classPath); ///< m-file
|
||||
void matlab_mfile(const std::string& classPath) const; ///< m-file
|
||||
void matlab_wrapper(const std::string& classPath,
|
||||
const std::string& className, const std::string& nameSpace); ///< wrapper
|
||||
const std::string& className,
|
||||
const std::string& cppClassName,
|
||||
const std::string& matlabClassname,
|
||||
const std::vector<std::string>& using_namespaces,
|
||||
const std::vector<std::string>& includes) const; ///< cpp wrapper
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
||||
|
|
|
|||
227
wrap/Module.cpp
227
wrap/Module.cpp
|
|
@ -16,15 +16,19 @@
|
|||
|
||||
#include "Module.h"
|
||||
#include "utilities.h"
|
||||
#include "pop_actor.h"
|
||||
|
||||
//#define BOOST_SPIRIT_DEBUG
|
||||
#include <boost/spirit/include/classic_core.hpp>
|
||||
#include <boost/spirit/include/classic_confix.hpp>
|
||||
#include <boost/spirit/include/classic_clear_actor.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
using namespace BOOST_SPIRIT_CLASSIC_NS;
|
||||
|
||||
typedef rule<BOOST_SPIRIT_CLASSIC_NS::phrase_scanner_t> Rule;
|
||||
|
|
@ -38,102 +42,154 @@ typedef rule<BOOST_SPIRIT_CLASSIC_NS::phrase_scanner_t> Rule;
|
|||
/* ************************************************************************* */
|
||||
|
||||
Module::Module(const string& interfacePath,
|
||||
const string& moduleName, bool verbose) : name(moduleName), verbose_(verbose)
|
||||
const string& moduleName, bool enable_verbose) : name(moduleName), verbose(enable_verbose)
|
||||
{
|
||||
// these variables will be imperatively updated to gradually build [cls]
|
||||
// The one with postfix 0 are used to reset the variables after parse.
|
||||
ReturnValue retVal0, retVal;
|
||||
Argument arg0, arg;
|
||||
ArgumentList args0, args;
|
||||
Constructor constructor0(verbose), constructor(verbose);
|
||||
Method method0(verbose), method(verbose);
|
||||
Class cls0(verbose),cls(verbose);
|
||||
Constructor constructor0(enable_verbose), constructor(enable_verbose);
|
||||
Method method0(enable_verbose), method(enable_verbose);
|
||||
StaticMethod static_method0(enable_verbose), static_method(enable_verbose);
|
||||
Class cls0(enable_verbose),cls(enable_verbose);
|
||||
vector<string> namespaces, namespaces_return;
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// Grammar with actions that build the Class object. Actions are
|
||||
// defined within the square brackets [] and are executed whenever a
|
||||
// rule is successfully parsed. Define BOOST_SPIRIT_DEBUG to debug.
|
||||
// The grammar is allows a very restricted C++ header:
|
||||
// - No comments allowed.
|
||||
// -Only types allowed are string, bool, size_t int, double, Vector, and Matrix
|
||||
// as well as class names that start with an uppercase letter
|
||||
// - The types unsigned int and bool should be specified as int.
|
||||
// The grammar is allows a very restricted C++ header
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p);
|
||||
|
||||
// lexeme_d turns off white space skipping
|
||||
// http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html
|
||||
|
||||
Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')];
|
||||
Rule basisType_p =
|
||||
(str_p("string") | "bool" | "size_t" | "int" | "double");
|
||||
|
||||
Rule keywords_p =
|
||||
(str_p("const") | "static" | "namespace" | basisType_p);
|
||||
|
||||
Rule eigenType_p =
|
||||
(str_p("Vector") | "Matrix");
|
||||
|
||||
Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p;
|
||||
|
||||
Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p;
|
||||
|
||||
Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_p("::");
|
||||
|
||||
Rule classPtr_p =
|
||||
*namespace_arg_p >>
|
||||
className_p [assign_a(arg.type)] >>
|
||||
ch_p('*') [assign_a(arg.is_ptr,true)];
|
||||
|
||||
Rule classRef_p =
|
||||
!str_p("const") [assign_a(arg.is_const,true)] >>
|
||||
*namespace_arg_p >>
|
||||
className_p [assign_a(arg.type)] >>
|
||||
ch_p('&') [assign_a(arg.is_ref,true)];
|
||||
|
||||
Rule basisType_p =
|
||||
(str_p("string") | "bool" | "size_t" | "int" | "double");
|
||||
Rule argEigenType_p =
|
||||
eigenType_p[assign_a(arg.type)] >>
|
||||
!ch_p('*')[assign_a(arg.is_ptr,true)];
|
||||
|
||||
Rule ublasType =
|
||||
(str_p("Vector") | "Matrix")[assign_a(arg.type)] >>
|
||||
!ch_p('*')[assign_a(arg.is_ptr,true)];
|
||||
Rule eigenRef_p =
|
||||
!str_p("const") [assign_a(arg.is_const,true)] >>
|
||||
eigenType_p [assign_a(arg.type)] >>
|
||||
ch_p('&') [assign_a(arg.is_ref,true)];
|
||||
|
||||
Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
|
||||
|
||||
Rule argument_p =
|
||||
((basisType_p[assign_a(arg.type)] | ublasType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)])
|
||||
((basisType_p[assign_a(arg.type)] | argEigenType_p | classRef_p | eigenRef_p | classPtr_p)
|
||||
>> name_p[assign_a(arg.name)])
|
||||
[push_back_a(args, arg)]
|
||||
[assign_a(arg,arg0)];
|
||||
|
||||
Rule argumentList_p = !argument_p >> * (',' >> argument_p);
|
||||
|
||||
Rule constructor_p =
|
||||
(className_p >> '(' >> argumentList_p >> ')' >> ';')
|
||||
(className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p)
|
||||
[assign_a(constructor.args,args)]
|
||||
[assign_a(constructor.name,cls.name)]
|
||||
[assign_a(args,args0)]
|
||||
[push_back_a(cls.constructors, constructor)]
|
||||
[assign_a(constructor,constructor0)];
|
||||
|
||||
Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::");
|
||||
|
||||
Rule returnType1_p =
|
||||
basisType_p[assign_a(method.returns_)] |
|
||||
((className_p | "Vector" | "Matrix")[assign_a(method.returns_)] >>
|
||||
!ch_p('*') [assign_a(method.returns_ptr_,true)]);
|
||||
(basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::BASIS)]) |
|
||||
((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)]
|
||||
>> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::CLASS)]) >>
|
||||
!ch_p('*')[assign_a(retVal.isPtr1,true)]) |
|
||||
(eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::EIGEN)]);
|
||||
|
||||
Rule returnType2_p =
|
||||
basisType_p[assign_a(method.returns2_)] |
|
||||
((className_p | "Vector" | "Matrix")[assign_a(method.returns2_)] >>
|
||||
!ch_p('*') [assign_a(method.returns_ptr2_,true)]);
|
||||
(basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::BASIS)]) |
|
||||
((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)]
|
||||
>> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::CLASS)]) >>
|
||||
!ch_p('*') [assign_a(retVal.isPtr2,true)]) |
|
||||
(eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::EIGEN)]);
|
||||
|
||||
Rule pair_p =
|
||||
(str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>')
|
||||
[assign_a(method.returns_pair_,true)];
|
||||
[assign_a(retVal.isPair,true)];
|
||||
|
||||
Rule void_p = str_p("void")[assign_a(method.returns_)];
|
||||
Rule void_p = str_p("void")[assign_a(retVal.type1)];
|
||||
|
||||
Rule returnType_p = void_p | returnType1_p | pair_p;
|
||||
|
||||
Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')];
|
||||
|
||||
Rule method_p =
|
||||
(returnType_p >> methodName_p[assign_a(method.name_)] >>
|
||||
(returnType_p >> methodName_p[assign_a(method.name)] >>
|
||||
'(' >> argumentList_p >> ')' >>
|
||||
!str_p("const")[assign_a(method.is_const_,true)] >> ';')
|
||||
[assign_a(method.args_,args)]
|
||||
!str_p("const")[assign_a(method.is_const_,true)] >> ';' >> *comments_p)
|
||||
[assign_a(method.args,args)]
|
||||
[assign_a(args,args0)]
|
||||
[assign_a(method.returnVal,retVal)]
|
||||
[assign_a(retVal,retVal0)]
|
||||
[push_back_a(cls.methods, method)]
|
||||
[assign_a(method,method0)];
|
||||
|
||||
Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >>
|
||||
*constructor_p >>
|
||||
*method_p >>
|
||||
'}' >> ";";
|
||||
Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
|
||||
|
||||
Rule module_p = +class_p
|
||||
[push_back_a(classes,cls)]
|
||||
[assign_a(cls,cls0)]
|
||||
>> !end_p;
|
||||
Rule static_method_p =
|
||||
(str_p("static") >> returnType_p >> staticMethodName_p[assign_a(static_method.name)] >>
|
||||
'(' >> argumentList_p >> ')' >> ';' >> *comments_p)
|
||||
[assign_a(static_method.args,args)]
|
||||
[assign_a(args,args0)]
|
||||
[assign_a(static_method.returnVal,retVal)]
|
||||
[assign_a(retVal,retVal0)]
|
||||
[push_back_a(cls.static_methods, static_method)]
|
||||
[assign_a(static_method,static_method0)];
|
||||
|
||||
Rule includes_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(cls.includes)] >> ch_p('>');
|
||||
|
||||
Rule functions_p = includes_p | constructor_p | method_p | static_method_p;
|
||||
|
||||
Rule class_p = (str_p("class") >> className_p[assign_a(cls.name)] >> '{' >>
|
||||
*(functions_p | comments_p) >>
|
||||
str_p("};"))[assign_a(cls.namespaces, namespaces)][push_back_a(classes,cls)][assign_a(cls,cls0)];
|
||||
|
||||
Rule namespace_def_p = str_p("namespace") >>
|
||||
namespace_name_p[push_back_a(namespaces)]
|
||||
>> ch_p('{') >>
|
||||
*(class_p | namespace_def_p | comments_p) >>
|
||||
str_p("}///\\namespace") >> !namespace_name_p // end namespace, avoid confusion with classes
|
||||
[pop_a(namespaces)];
|
||||
|
||||
Rule using_namespace_p = str_p("using") >> str_p("namespace")
|
||||
>> namespace_name_p[push_back_a(using_namespaces)] >> ch_p(';');
|
||||
|
||||
Rule module_content_p = comments_p | using_namespace_p | class_p | namespace_def_p ;
|
||||
|
||||
Rule module_p = *module_content_p >> !end_p;
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// for debugging, define BOOST_SPIRIT_DEBUG
|
||||
|
|
@ -162,34 +218,45 @@ Module::Module(const string& interfacePath,
|
|||
string interfaceFile = interfacePath + "/" + moduleName + ".h";
|
||||
string contents = file_contents(interfaceFile);
|
||||
|
||||
// Comment parser : does not work for some reason
|
||||
rule<> comment_p = str_p("/*") >> +anychar_p >> "*/";
|
||||
rule<> skip_p = space_p; // | comment_p;
|
||||
|
||||
// and parse contents
|
||||
parse_info<const char*> info = parse(contents.c_str(), module_p, skip_p);
|
||||
parse_info<const char*> info = parse(contents.c_str(), module_p, space_p);
|
||||
if(!info.full) {
|
||||
printf("parsing stopped at \n%.20s\n",info.stop);
|
||||
throw ParseFailed(info.length);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class T>
|
||||
void verifyArguments(const vector<string>& validArgs, const vector<T>& vt) {
|
||||
BOOST_FOREACH(const T& t, vt) {
|
||||
BOOST_FOREACH(Argument arg, t.args) {
|
||||
string fullType = arg.qualifiedType("::");
|
||||
if(find(validArgs.begin(), validArgs.end(), fullType)
|
||||
== validArgs.end())
|
||||
throw DependencyMissing(fullType, t.name);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Module::matlab_code(const string& toolboxPath,
|
||||
const string& nameSpace,
|
||||
const string& mexFlags)
|
||||
{
|
||||
try {
|
||||
const string& mexExt, const string& mexFlags) const {
|
||||
string installCmd = "install -d " + toolboxPath;
|
||||
system(installCmd.c_str());
|
||||
|
||||
// create make m-file
|
||||
string makeFile = toolboxPath + "/make_" + name + ".m";
|
||||
ofstream ofs(makeFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(makeFile);
|
||||
string matlabMakeFile = toolboxPath + "/make_" + name + ".m";
|
||||
ofstream ofs(matlabMakeFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(matlabMakeFile);
|
||||
|
||||
if (verbose_) cerr << "generating " << makeFile << endl;
|
||||
emit_header_comment(ofs,"%");
|
||||
// create the (actual) make file
|
||||
string makeFile = toolboxPath + "/Makefile";
|
||||
ofstream make_ofs(makeFile.c_str());
|
||||
if(!make_ofs) throw CantOpenFile(makeFile);
|
||||
|
||||
if (verbose) cerr << "generating " << matlabMakeFile << endl;
|
||||
generateHeaderComment(ofs,"%");
|
||||
ofs << "echo on" << endl << endl;
|
||||
ofs << "toolboxpath = mfilename('fullpath');" << endl;
|
||||
ofs << "delims = find(toolboxpath == '/');" << endl;
|
||||
|
|
@ -197,36 +264,76 @@ void Module::matlab_code(const string& toolboxPath,
|
|||
ofs << "clear delims" << endl;
|
||||
ofs << "addpath(toolboxpath);" << endl << endl;
|
||||
|
||||
if (verbose) cerr << "generating " << makeFile << endl;
|
||||
generateHeaderComment(make_ofs,"#");
|
||||
make_ofs << "\nMEX = mex\n";
|
||||
make_ofs << "MEXENDING = " << mexExt << "\n";
|
||||
make_ofs << "mex_flags = " << mexFlags << "\n\n";
|
||||
|
||||
//Dependency check list
|
||||
vector<string> validArgs;
|
||||
validArgs.push_back("string");
|
||||
validArgs.push_back("int");
|
||||
validArgs.push_back("bool");
|
||||
validArgs.push_back("size_t");
|
||||
validArgs.push_back("double");
|
||||
validArgs.push_back("Vector");
|
||||
validArgs.push_back("Matrix");
|
||||
|
||||
// add 'all' to Makefile
|
||||
make_ofs << "all: ";
|
||||
BOOST_FOREACH(Class cls, classes) {
|
||||
make_ofs << cls.qualifiedName() << " ";
|
||||
//Create a list of parsed classes for dependency checking
|
||||
validArgs.push_back(cls.qualifiedName("::"));
|
||||
}
|
||||
make_ofs << "\n\n";
|
||||
|
||||
// generate proxy classes and wrappers
|
||||
BOOST_FOREACH(Class cls, classes) {
|
||||
// create directory if needed
|
||||
string classPath = toolboxPath + "/@" + cls.name;
|
||||
string classPath = toolboxPath + "/@" + cls.qualifiedName();
|
||||
string installCmd = "install -d " + classPath;
|
||||
system(installCmd.c_str());
|
||||
|
||||
// create proxy class
|
||||
string classFile = classPath + "/" + cls.name + ".m";
|
||||
string classFile = classPath + "/" + cls.qualifiedName() + ".m";
|
||||
cls.matlab_proxy(classFile);
|
||||
|
||||
// verify all of the function arguments
|
||||
verifyArguments<Constructor>(validArgs, cls.constructors);
|
||||
verifyArguments<StaticMethod>(validArgs, cls.static_methods);
|
||||
verifyArguments<Method>(validArgs, cls.methods);
|
||||
|
||||
// create constructor and method wrappers
|
||||
cls.matlab_constructors(toolboxPath,nameSpace);
|
||||
cls.matlab_methods(classPath,nameSpace);
|
||||
cls.matlab_constructors(toolboxPath,using_namespaces);
|
||||
cls.matlab_static_methods(toolboxPath,using_namespaces);
|
||||
cls.matlab_methods(classPath,using_namespaces);
|
||||
|
||||
// add lines to make m-file
|
||||
ofs << "%% " << cls.name << endl;
|
||||
ofs << "%% " << cls.qualifiedName() << endl;
|
||||
ofs << "cd(toolboxpath)" << endl;
|
||||
cls.matlab_make_fragment(ofs, toolboxPath, mexFlags);
|
||||
|
||||
// add section to the (actual) make file
|
||||
make_ofs << "# " << cls.qualifiedName() << endl;
|
||||
cls.makefile_fragment(make_ofs);
|
||||
}
|
||||
|
||||
// finish make m-file
|
||||
ofs << "cd(toolboxpath)" << endl << endl;
|
||||
ofs << "echo off" << endl;
|
||||
ofs.close();
|
||||
}
|
||||
catch(exception &e) {
|
||||
cerr << "generate_matlab_toolbox failed because " << e.what() << endl;
|
||||
}
|
||||
|
||||
}
|
||||
// make clean at end of Makefile
|
||||
make_ofs << "\n\nclean: \n";
|
||||
make_ofs << "\trm -rf *.$(MEXENDING)\n";
|
||||
BOOST_FOREACH(Class cls, classes)
|
||||
make_ofs << "\trm -rf @" << cls.qualifiedName() << "/*.$(MEXENDING)\n";
|
||||
|
||||
// finish Makefile
|
||||
make_ofs << "\n" << endl;
|
||||
make_ofs.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -18,26 +18,30 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
#include "Class.h"
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/**
|
||||
* A module just has a name and a list of classes
|
||||
*/
|
||||
struct Module {
|
||||
std::string name; ///< module name
|
||||
std::list<Class> classes; ///< list of classes
|
||||
bool verbose_; ///< verbose flag
|
||||
std::vector<Class> classes; ///< list of classes
|
||||
bool verbose; ///< verbose flag
|
||||
std::vector<std::string> using_namespaces; ///< all default namespaces
|
||||
|
||||
/// constructor that parses interface file
|
||||
Module(const std::string& interfacePath,
|
||||
const std::string& moduleName,
|
||||
bool verbose=true);
|
||||
bool enable_verbose=true);
|
||||
|
||||
/// MATLAB code generation:
|
||||
void matlab_code(const std::string& path,
|
||||
const std::string& nameSpace,
|
||||
const std::string& mexFlags);
|
||||
const std::string& mexExt,
|
||||
const std::string& mexFlags) const;
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
|
|
|||
|
|
@ -17,10 +17,10 @@ OBJECT CREATION
|
|||
new_GaussianFactorGraph_ calls wrap_constructed in matlab.h, see documentation there
|
||||
|
||||
METHOD (AND CONSTRUCTOR) ARGUMENTS
|
||||
- simple argument types of methods, such as "double", will be converted in the
|
||||
- Simple argument types of methods, such as "double", will be converted in the
|
||||
mex warppers by calling unwrap<double>, defined in matlab.h
|
||||
- Vector and Matric arguments are normally passed by reference in GTSAM, but
|
||||
in gtsam.h you need to pretedn they are passed by value, to trigger the
|
||||
- Vector and Matrix arguments are normally passed by reference in GTSAM, but
|
||||
in gtsam.h you need to pretend they are passed by value, to trigger the
|
||||
generation of the correct conversion routines unwrap<Vector> and unwrap<Matrix>
|
||||
- passing classes as arguments works, provided they are passed by reference.
|
||||
This triggers a call to unwrap_shared_ptr
|
||||
|
|
|
|||
|
|
@ -0,0 +1,78 @@
|
|||
/**
|
||||
* @file ReturnValue.cpp
|
||||
*
|
||||
* @date Dec 1, 2011
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include "ReturnValue.h"
|
||||
#include "utilities.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ReturnValue::return_type(bool add_ptr, pairing p) const {
|
||||
if (p==pair && isPair) {
|
||||
string str = "pair< " +
|
||||
maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::")) + ", " +
|
||||
maybe_shared_ptr(add_ptr && isPtr2, qualifiedType2("::")) + " >";
|
||||
return str;
|
||||
} else
|
||||
return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ReturnValue::matlab_returnType() const {
|
||||
return isPair? "[first,second]" : "result";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ReturnValue::qualifiedType1(const string& delim) const {
|
||||
string result;
|
||||
BOOST_FOREACH(const string& ns, namespaces1) result += ns + delim;
|
||||
return result + type1;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ReturnValue::qualifiedType2(const string& delim) const {
|
||||
string result;
|
||||
BOOST_FOREACH(const string& ns, namespaces2) result += ns + delim;
|
||||
return result + type2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ReturnValue::wrap_result(ostream& ofs) const {
|
||||
string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1();
|
||||
string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2();
|
||||
|
||||
if (isPair) {
|
||||
// first return value in pair
|
||||
if (isPtr1) // if we already have a pointer
|
||||
ofs << " out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n";
|
||||
else if (category1 == ReturnValue::CLASS) // if we are going to make one
|
||||
ofs << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result.first),\"" << matlabType1 << "\");\n";
|
||||
else // if basis type
|
||||
ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n";
|
||||
|
||||
// second return value in pair
|
||||
if (isPtr2) // if we already have a pointer
|
||||
ofs << " out[1] = wrap_shared_ptr(result.second,\"" << type2 << "\");\n";
|
||||
else if (category2 == ReturnValue::CLASS) // if we are going to make one
|
||||
ofs << " out[1] = wrap_shared_ptr(make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n";
|
||||
else
|
||||
ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
|
||||
}
|
||||
else if (isPtr1)
|
||||
ofs << " out[0] = wrap_shared_ptr(result,\"" << type1 << "\");\n";
|
||||
else if (category1 == ReturnValue::CLASS)
|
||||
ofs << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\n";
|
||||
else if (type1!="void")
|
||||
ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
/**
|
||||
* @file ReturnValue.h
|
||||
*
|
||||
* @brief Encapsulates a return value from a method
|
||||
*
|
||||
* @date Dec 1, 2011
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <vector>
|
||||
#include <ostream>
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace wrap {
|
||||
|
||||
struct ReturnValue {
|
||||
|
||||
typedef enum {
|
||||
CLASS,
|
||||
EIGEN,
|
||||
BASIS,
|
||||
VOID
|
||||
} return_category;
|
||||
|
||||
ReturnValue(bool enable_verbosity = true)
|
||||
: verbose(enable_verbosity), isPtr1(false), isPtr2(false),
|
||||
isPair(false), category1(VOID), category2(VOID)
|
||||
{}
|
||||
|
||||
bool verbose;
|
||||
std::string type1, type2;
|
||||
bool isPtr1, isPtr2, isPair;
|
||||
std::vector<std::string> namespaces1, namespaces2;
|
||||
|
||||
return_category category1, category2;
|
||||
|
||||
typedef enum {
|
||||
arg1, arg2, pair
|
||||
} pairing;
|
||||
|
||||
std::string return_type(bool add_ptr, pairing p) const;
|
||||
|
||||
std::string qualifiedType1(const std::string& delim = "") const;
|
||||
std::string qualifiedType2(const std::string& delim = "") const;
|
||||
|
||||
std::string matlab_returnType() const;
|
||||
|
||||
void wrap_result(std::ostream& ofs) const;
|
||||
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Method.ccp
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include "StaticMethod.h"
|
||||
#include "utilities.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) const {
|
||||
|
||||
// open destination m-file
|
||||
string full_name = className + "_" + name;
|
||||
string wrapperFile = toolboxPath + "/" + full_name + ".m";
|
||||
ofstream ofs(wrapperFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(wrapperFile);
|
||||
if(verbose) cerr << "generating " << wrapperFile << endl;
|
||||
|
||||
// generate code
|
||||
string returnType = returnVal.matlab_returnType();
|
||||
ofs << "function " << returnType << " = " << full_name << "(";
|
||||
if (args.size()) ofs << args.names();
|
||||
ofs << ")" << endl;
|
||||
ofs << "% usage: x = " << full_name << "(" << args.names() << ")" << endl;
|
||||
ofs << " error('need to compile " << full_name << ".cpp');" << endl;
|
||||
ofs << "end" << endl;
|
||||
|
||||
// close file
|
||||
ofs.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className,
|
||||
const string& matlabClassName, const string& cppClassName,
|
||||
const vector<string>& using_namespaces,
|
||||
const vector<string>& includes) const {
|
||||
// open destination wrapperFile
|
||||
string full_name = matlabClassName + "_" + name;
|
||||
string wrapperFile = toolboxPath + "/" + full_name + ".cpp";
|
||||
ofstream ofs(wrapperFile.c_str());
|
||||
if(!ofs) throw CantOpenFile(wrapperFile);
|
||||
if(verbose) cerr << "generating " << wrapperFile << endl;
|
||||
|
||||
// generate code
|
||||
|
||||
// header
|
||||
generateHeaderComment(ofs, "//");
|
||||
generateIncludes(ofs, className, includes);
|
||||
generateUsingNamespace(ofs, using_namespaces);
|
||||
|
||||
// call
|
||||
ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
|
||||
// start
|
||||
ofs << "{\n";
|
||||
|
||||
// check arguments
|
||||
// NOTE: for static functions, there is no object passed
|
||||
ofs << " checkArguments(\"" << full_name << "\",nargout,nargin," << args.size() << ");\n";
|
||||
|
||||
// unwrap arguments, see Argument.cpp
|
||||
args.matlab_unwrap(ofs,0); // We start at 0 because there is no self object
|
||||
|
||||
ofs << " ";
|
||||
|
||||
// call method with default type
|
||||
if (returnVal.type1!="void")
|
||||
ofs << returnVal.return_type(true,ReturnValue::pair) << " result = ";
|
||||
ofs << cppClassName << "::" << name << "(" << args.names() << ");\n";
|
||||
|
||||
// wrap result
|
||||
// example: out[0]=wrap<bool>(result);
|
||||
returnVal.wrap_result(ofs);
|
||||
|
||||
// finish
|
||||
ofs << "}\n";
|
||||
|
||||
// close file
|
||||
ofs.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 StaticMethod.h
|
||||
* @brief describes and generates code for static methods
|
||||
* @author Frank Dellaert
|
||||
* @author Alex Cunningham
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <list>
|
||||
|
||||
#include "Argument.h"
|
||||
#include "ReturnValue.h"
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/// StaticMethod class
|
||||
struct StaticMethod {
|
||||
|
||||
/// Constructor creates empty object
|
||||
StaticMethod(bool verbosity = true) :
|
||||
verbose(verbosity) {}
|
||||
|
||||
// Then the instance variables are set directly by the Module constructor
|
||||
bool verbose;
|
||||
std::string name;
|
||||
ArgumentList args;
|
||||
ReturnValue returnVal;
|
||||
|
||||
// MATLAB code generation
|
||||
// toolboxPath is the core toolbox directory, e.g., ../matlab
|
||||
// NOTE: static functions are not inside the class, and
|
||||
// are created with [ClassName]_[FunctionName]() format
|
||||
|
||||
void matlab_mfile(const std::string& toolboxPath, const std::string& className) const; ///< m-file
|
||||
void matlab_wrapper(const std::string& toolboxPath,
|
||||
const std::string& className, const std::string& matlabClassName,
|
||||
const std::string& cppClassName,
|
||||
const std::vector<std::string>& using_namespaces,
|
||||
const std::vector<std::string>& includes) const; ///< cpp wrapper
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
|
||||
class Point2 {
|
||||
Point2();
|
||||
Point2(double x, double y);
|
||||
double x();
|
||||
double y();
|
||||
int dim() const;
|
||||
};
|
||||
|
||||
class Point3 {
|
||||
Point3(double x, double y, double z);
|
||||
double norm() const;
|
||||
};
|
||||
|
||||
class Test {
|
||||
Test();
|
||||
|
||||
bool return_bool (bool value);
|
||||
size_t return_size_t (size_t value);
|
||||
int return_int (int value);
|
||||
double return_double (double value);
|
||||
|
||||
string return_string (string value);
|
||||
Vector return_vector1(Vector value);
|
||||
Matrix return_matrix1(Matrix value);
|
||||
Vector return_vector2(Vector value);
|
||||
Matrix return_matrix2(Matrix value);
|
||||
|
||||
pair<Vector,Matrix> return_pair (Vector v, Matrix A);
|
||||
|
||||
bool return_field(const Test& t) const;
|
||||
|
||||
Test* return_TestPtr(Test* value);
|
||||
|
||||
pair<Test*,Test*> create_ptrs ();
|
||||
pair<Test*,Test*> return_ptrs (Test* p1, Test* p2);
|
||||
|
||||
void print();
|
||||
};
|
||||
|
|
@ -27,6 +27,7 @@ extern "C" {
|
|||
}
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#include <list>
|
||||
#include <string>
|
||||
|
|
@ -36,9 +37,6 @@ using namespace std;
|
|||
using namespace boost; // not usual, but for conciseness of generated code
|
||||
|
||||
// start GTSAM Specifics /////////////////////////////////////////////////
|
||||
//typedef gtsam::Vector Vector; // NOTE: outside of gtsam namespace
|
||||
//typedef gtsam::Matrix Matrix;
|
||||
|
||||
// to make keys be constructed from strings:
|
||||
#define GTSAM_MAGIC_KEY
|
||||
// to enable Matrix and Vector constructor for SharedGaussian:
|
||||
|
|
|
|||
|
|
@ -0,0 +1,59 @@
|
|||
/**
|
||||
* @file pop_actor.h
|
||||
*
|
||||
* @brief An actor to pop a vector/container, based off of the clear_actor
|
||||
*
|
||||
* @date Dec 8, 2011
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/spirit/include/classic_ref_actor.hpp>
|
||||
|
||||
namespace boost { namespace spirit {
|
||||
|
||||
BOOST_SPIRIT_CLASSIC_NAMESPACE_BEGIN
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////
|
||||
// Summary:
|
||||
// A semantic action policy that calls pop_back method.
|
||||
// (This doc uses convention available in actors.hpp)
|
||||
//
|
||||
// Actions (what it does):
|
||||
// ref.pop_back();
|
||||
//
|
||||
// Policy name:
|
||||
// clear_action
|
||||
//
|
||||
// Policy holder, corresponding helper method:
|
||||
// ref_actor, clear_a( ref );
|
||||
//
|
||||
// () operators: both.
|
||||
//
|
||||
// See also ref_actor for more details.
|
||||
///////////////////////////////////////////////////////////////////////////
|
||||
struct pop_action
|
||||
{
|
||||
template<
|
||||
typename T
|
||||
>
|
||||
void act(T& ref_) const
|
||||
{
|
||||
ref_.pop_back();
|
||||
}
|
||||
};
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////
|
||||
// helper method that creates a and_assign_actor.
|
||||
///////////////////////////////////////////////////////////////////////////
|
||||
template<typename T>
|
||||
inline ref_actor<T,pop_action> pop_a(T& ref_)
|
||||
{
|
||||
return ref_actor<T,pop_action>(ref_);
|
||||
}
|
||||
|
||||
BOOST_SPIRIT_CLASSIC_NAMESPACE_END
|
||||
|
||||
}}
|
||||
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
// automatically generated by wrap on 2011-Oct-31
|
||||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <Point2.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("dim",nargout,nargin-1,0);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
function result = dim(obj)
|
||||
% usage: obj.dim()
|
||||
% automatically generated by wrap on 2011-Oct-31
|
||||
error('need to compile dim.cpp');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -0,0 +1,11 @@
|
|||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <Point2.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("vectorConfusion",nargout,nargin-1,0);
|
||||
shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
|
||||
VectorNotEigen result = self->vectorConfusion();
|
||||
out[0] = wrap_shared_ptr(make_shared< VectorNotEigen >(result),"VectorNotEigen");
|
||||
}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
function result = vectorConfusion(obj)
|
||||
% usage: obj.vectorConfusion()
|
||||
error('need to compile vectorConfusion.cpp');
|
||||
end
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
// automatically generated by wrap on 2011-Oct-31
|
||||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <Point2.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("x",nargout,nargin-1,0);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
function result = x(obj)
|
||||
% usage: obj.x()
|
||||
% automatically generated by wrap on 2011-Oct-31
|
||||
error('need to compile x.cpp');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
// automatically generated by wrap on 2011-Oct-31
|
||||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <Point2.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("y",nargout,nargin-1,0);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
function result = y(obj)
|
||||
% usage: obj.y()
|
||||
% automatically generated by wrap on 2011-Oct-31
|
||||
error('need to compile y.cpp');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
// automatically generated by wrap on 2011-Oct-31
|
||||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <Point3.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("norm",nargout,nargin-1,0);
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@ classdef Test
|
|||
methods
|
||||
function obj = Test(varargin)
|
||||
if nargin == 0, obj.self = new_Test_(); end
|
||||
if nargin == 2, obj.self = new_Test_dM(varargin{1},varargin{2}); end
|
||||
if nargin ~= 13 && obj.self == 0, error('Test constructor failed'); end
|
||||
end
|
||||
function display(obj), obj.print(''); end
|
||||
|
|
|
|||
|
|
@ -0,0 +1,11 @@
|
|||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <folder/path/to/Test.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
|
||||
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
|
||||
Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix");
|
||||
self->arg_EigenConstRef(value);
|
||||
}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
function result = arg_EigenConstRef(obj,value)
|
||||
% usage: obj.arg_EigenConstRef(value)
|
||||
error('need to compile arg_EigenConstRef.cpp');
|
||||
end
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <folder/path/to/Test.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("create_MixedPtrs",nargout,nargin-1,0);
|
||||
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
|
||||
pair< Test, shared_ptr<Test> > result = self->create_MixedPtrs();
|
||||
out[0] = wrap_shared_ptr(make_shared< Test >(result.first),"Test");
|
||||
out[1] = wrap_shared_ptr(result.second,"Test");
|
||||
}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
function [first,second] = create_MixedPtrs(obj)
|
||||
% usage: obj.create_MixedPtrs()
|
||||
error('need to compile create_MixedPtrs.cpp');
|
||||
end
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
// automatically generated by wrap on 2011-Oct-31
|
||||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <Test.h>
|
||||
#include <folder/path/to/Test.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("create_ptrs",nargout,nargin-1,0);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
function [first,second] = create_ptrs(obj)
|
||||
% usage: obj.create_ptrs()
|
||||
% automatically generated by wrap on 2011-Oct-31
|
||||
error('need to compile create_ptrs.cpp');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
// automatically generated by wrap on 2011-Oct-31
|
||||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <Test.h>
|
||||
#include <folder/path/to/Test.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("print",nargout,nargin-1,0);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,12 @@
|
|||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <folder/path/to/Test.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("return_Point2Ptr",nargout,nargin-1,1);
|
||||
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
|
||||
bool value = unwrap< bool >(in[1]);
|
||||
shared_ptr<Point2> result = self->return_Point2Ptr(value);
|
||||
out[0] = wrap_shared_ptr(result,"Point2");
|
||||
}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
function result = return_Point2Ptr(obj,value)
|
||||
% usage: obj.return_Point2Ptr(value)
|
||||
error('need to compile return_Point2Ptr.cpp');
|
||||
end
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <folder/path/to/Test.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("return_Test",nargout,nargin-1,1);
|
||||
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
|
||||
shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "Test");
|
||||
Test result = self->return_Test(value);
|
||||
out[0] = wrap_shared_ptr(make_shared< Test >(result),"Test");
|
||||
}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
function result = return_Test(obj,value)
|
||||
% usage: obj.return_Test(value)
|
||||
error('need to compile return_Test.cpp');
|
||||
end
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
// automatically generated by wrap on 2011-Oct-31
|
||||
// automatically generated by wrap on 2011-Dec-09
|
||||
#include <wrap/matlab.h>
|
||||
#include <Test.h>
|
||||
#include <folder/path/to/Test.h>
|
||||
using namespace geometry;
|
||||
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("return_TestPtr",nargout,nargin-1,1);
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue