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

(in branch) Merged from trunk r7960-r8057

release/4.3a0
Richard Roberts 2011-12-12 16:03:52 +00:00
parent fd5b040385
commit 3b139cbae2
174 changed files with 3137 additions and 1518 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
.cproject
View File

@ -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="&quot;${workspace_loc:/gtsam}&quot;"/>
<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
View File

@ -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 \

2
examples/Data/calib.txt
View File

@ -1 +1 @@
800 600 -1119.61507797 1119.61507797 399.5 299.5
800 600 1119.61507797 1119.61507797 399.5 299.5

12
examples/Data/measurementsISAM.txt
View File

@ -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

12
examples/Data/posesISAM.txt
View File

@ -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

8
examples/Data/ttpy10.pose
View File

@ -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

8
examples/Data/ttpy100.pose
View File

@ -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

8
examples/Data/ttpy20.pose
View File

@ -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

8
examples/Data/ttpy30.pose
View File

@ -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

8
examples/Data/ttpy40.pose
View File

@ -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

8
examples/Data/ttpy50.pose
View File

@ -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

8
examples/Data/ttpy60.pose
View File

@ -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

8
examples/Data/ttpy70.pose
View File

@ -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

8
examples/Data/ttpy80.pose
View File

@ -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

8
examples/Data/ttpy90.pose
View File

@ -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

24
examples/vSLAMexample/vISAMexample.cpp
View File

@ -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();

49
examples/vSLAMexample/vSLAMutils.cpp
View File

@ -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;

4
examples/vSLAMexample/vSLAMutils.h
View File

@ -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
View File

@ -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
View File

@ -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;
//};

12
gtsam/base/TestableAssertions.h
View File

@ -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
*/

17
gtsam/base/cholesky.cpp
View File

@ -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();
}

10
gtsam/geometry/Pose2.h
View File

@ -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
*/

7
gtsam/geometry/Pose3.cpp
View File

@ -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();
@ -216,7 +216,8 @@ namespace gtsam {
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
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();

8
gtsam/geometry/Pose3.h
View File

@ -105,10 +105,10 @@ namespace gtsam {
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:

146
gtsam/geometry/Rot3M.cpp
View File

@ -25,40 +25,41 @@ 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) {
/* ************************************************************************* */
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) {
/* ************************************************************************* */
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) {
/* ************************************************************************* */
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) {
/* ************************************************************************* */
// 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);
@ -78,10 +79,10 @@ namespace gtsam {
_cs, c_c + sss, -s_c + css,
-sy, sc_, cc_
);
}
}
/* ************************************************************************* */
Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
/* ************************************************************************* */
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;
@ -97,70 +98,78 @@ namespace gtsam {
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*wwTzz;
return Rot3M( c + C00, -swz + C01, swy + C02,
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) {
/* ************************************************************************* */
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 {
/* ************************************************************************* */
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(),
/* ************************************************************************* */
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 Matrix_(3,3, r);
}
r1_.z(), r2_.z(), r3_.z();
return R;
}
/* ************************************************************************* */
Matrix Rot3M::transpose() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
/* ************************************************************************* */
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 Matrix_(3,3, r);
}
r3_.x(), r3_.y(), r3_.z();
return Rt;
}
/* ************************************************************************* */
Point3 Rot3M::column(int index) const{
/* ************************************************************************* */
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 {
/* ************************************************************************* */
Vector Rot3M::xyz() const {
Matrix I;Vector q;
boost::tie(I,q)=RQ(matrix());
return q;
}
}
Vector Rot3M::ypr() const {
/* ************************************************************************* */
Vector Rot3M::ypr() const {
Vector q = xyz();
return Vector_(3,q(2),q(1),q(0));
}
}
Vector Rot3M::rpy() const {
/* ************************************************************************* */
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) {
/* ************************************************************************* */
// 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)
@ -170,6 +179,7 @@ namespace gtsam {
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())) *
@ -187,45 +197,45 @@ namespace gtsam {
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
}
}
}
/* ************************************************************************* */
Point3 Rot3M::rotate(const Point3& p,
/* ************************************************************************* */
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,
/* ************************************************************************* */
// 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,
/* ************************************************************************* */
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,
/* ************************************************************************* */
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) {
/* ************************************************************************* */
pair<Matrix, Vector> RQ(const Matrix& A) {
double x = -atan2(-A(2, 1), A(2, 2));
Rot3M Qx = Rot3M::Rx(-x);
@ -241,8 +251,8 @@ namespace gtsam {
Vector xyz = Vector_(3, x, y, z);
return make_pair(R, xyz);
}
}
/* ************************************************************************* */
/* ************************************************************************* */
} // namespace gtsam

12
gtsam/geometry/Rot3M.h
View File

@ -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
*/

92
gtsam/geometry/tests/testPose3.cpp
View File

@ -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));
}

5
gtsam/inference/BayesTree.h
View File

@ -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.

14
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -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;

7
gtsam/inference/Permutation.h
View File

@ -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); }

3
gtsam/linear/GaussianFactor.h
View File

@ -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

6
gtsam/linear/HessianFactor.h
View File

@ -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;

6
gtsam/linear/JacobianFactor.h
View File

@ -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;

6
gtsam/nonlinear/GaussianISAM2.h
View File

@ -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

23
gtsam/nonlinear/ISAM2.h
View File

@ -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:

27
gtsam/slam/Landmark2.h
View File

@ -1,27 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Landmark2.h
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/Point2.h>
namespace gtsam {
typedef gtsam::Point2 Landmark2;
}

20
gtsam/slam/Makefile.am
View File

@ -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

28
gtsam/slam/PlanarSLAMGraph.h
View File

@ -1,28 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PlanarSLAMGraph.h
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/PlanarSLAMValues.h>
namespace gtsam {
typedef gtsam::planarSLAM::Graph PlanarSLAMGraph;
}

28
gtsam/slam/PlanarSLAMOdometry.h
View File

@ -1,28 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PlanarSLAMOdometry.h
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/PlanarSLAMValues.h>
namespace gtsam {
typedef gtsam::planarSLAM::Odometry PlanarSLAMOdometry;
}

27
gtsam/slam/PlanarSLAMValues.h
View File

@ -1,27 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PlanarSLAMValues.h
* @brief Convenience for MATLAB wrapper, which does not allow for identically named methods
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/slam/planarSLAM.h>
namespace gtsam {
typedef gtsam::planarSLAM::Values PlanarSLAMValues;
}

28
gtsam/slam/Simulated2DMeasurement.h
View File

@ -1,28 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file 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;
}

28
gtsam/slam/Simulated2DOdometry.h
View File

@ -1,28 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file 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;
}

28
gtsam/slam/Simulated2DOrientedOdometry.h
View File

@ -1,28 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file 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;
}

29
gtsam/slam/Simulated2DOrientedPosePrior.h
View File

@ -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;
}

59
gtsam/slam/Simulated2DOrientedValues.h
View File

@ -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

29
gtsam/slam/Simulated2DPointPrior.h
View File

@ -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;
}

29
gtsam/slam/Simulated2DPosePrior.h
View File

@ -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;
}

58
gtsam/slam/Simulated2DValues.h
View File

@ -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

2
gtsam/slam/Simulated3D.cpp
View File

@ -15,7 +15,7 @@
* @author Alex Cunningham
**/
#include <gtsam/slam/Simulated3D.h>
#include <gtsam/slam/simulated3D.h>
namespace gtsam {

19
gtsam/slam/planarSLAM.h
View File

@ -71,10 +71,10 @@ namespace gtsam {
// Convenience for MATLAB wrapper, which does not allow for identically named methods
/// 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);
}
};

16
gtsam/slam/simulated2D.h
View File

@ -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

30
gtsam/slam/simulated2DOriented.h
View File

@ -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

43
gtsam/slam/simulated3D.cpp Normal file
View File

@ -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

132
gtsam/slam/simulated3D.h Normal file
View File

@ -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

2
gtsam/slam/tests/testSimulated3D.cpp
View File

@ -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;

13
myconfigure.matlab
View File

@ -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

10
tests/matlab/test_gtsam.m Normal file
View File

@ -0,0 +1,10 @@
% Test runner script - runs each test
display 'Starting: testJacobianFactor'
testJacobianFactor
display 'Starting: testKalmanFilter'
testKalmanFilter
% end of tests
display 'Tests complete!'

104
tests/testGaussianISAM2.cpp
View File

@ -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);}
/* ************************************************************************* */

33
wrap/Argument.cpp
View File

@ -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;

23
wrap/Argument.h
View File

@ -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
View File

@ -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;
}
/* ************************************************************************* */

25
wrap/Class.h
View File

@ -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 {
@ -30,18 +32,27 @@ struct Class {
// 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
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

42
wrap/Constructor.cpp
View File

@ -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

20
wrap/Constructor.h
View 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

87
wrap/Makefile.am
View File

@ -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:

79
wrap/Method.cpp
View File

@ -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";

30
wrap/Method.h
View File

@ -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
View File

@ -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 ublasType =
(str_p("Vector") | "Matrix")[assign_a(arg.type)] >>
Rule argEigenType_p =
eigenType_p[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();
}
/* ************************************************************************* */

16
wrap/Module.h
View File

@ -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

6
wrap/README
View File

@ -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

78
wrap/ReturnValue.cpp Normal file
View File

@ -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";
}
/* ************************************************************************* */

53
wrap/ReturnValue.h Normal file
View File

@ -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

100
wrap/StaticMethod.cpp Normal file
View File

@ -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();
}
/* ************************************************************************* */

56
wrap/StaticMethod.h Normal file
View File

@ -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

39
wrap/geometry.h
View File

@ -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();
};

4
wrap/matlab.h
View File

@ -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:

59
wrap/pop_actor.h Normal file
View File

@ -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
}}

3
wrap/tests/expected/@Point2/dim.cpp
View File

@ -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
wrap/tests/expected/@Point2/dim.m
View File

@ -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

11
wrap/tests/expected/@Point2/vectorConfusion.cpp Normal file
View File

@ -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");
}

4
wrap/tests/expected/@Point2/vectorConfusion.m Normal file
View File

@ -0,0 +1,4 @@
function result = vectorConfusion(obj)
% usage: obj.vectorConfusion()
error('need to compile vectorConfusion.cpp');
end

3
wrap/tests/expected/@Point2/x.cpp
View File

@ -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
wrap/tests/expected/@Point2/x.m
View File

@ -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

3
wrap/tests/expected/@Point2/y.cpp
View File

@ -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
wrap/tests/expected/@Point2/y.m
View File

@ -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

3
wrap/tests/expected/@Point3/norm.cpp
View File

@ -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);

1
wrap/tests/expected/@Test/Test.m
View File

@ -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

11
wrap/tests/expected/@Test/arg_EigenConstRef.cpp Normal file
View File

@ -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);
}

4
wrap/tests/expected/@Test/arg_EigenConstRef.m Normal file
View File

@ -0,0 +1,4 @@
function result = arg_EigenConstRef(obj,value)
% usage: obj.arg_EigenConstRef(value)
error('need to compile arg_EigenConstRef.cpp');
end

12
wrap/tests/expected/@Test/create_MixedPtrs.cpp Normal file
View File

@ -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");
}

4
wrap/tests/expected/@Test/create_MixedPtrs.m Normal file
View File

@ -0,0 +1,4 @@
function [first,second] = create_MixedPtrs(obj)
% usage: obj.create_MixedPtrs()
error('need to compile create_MixedPtrs.cpp');
end

5
wrap/tests/expected/@Test/create_ptrs.cpp
View File

@ -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
wrap/tests/expected/@Test/create_ptrs.m
View File

@ -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

5
wrap/tests/expected/@Test/print.cpp
View File

@ -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);

12
wrap/tests/expected/@Test/return_Point2Ptr.cpp Normal file
View File

@ -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");
}

4
wrap/tests/expected/@Test/return_Point2Ptr.m Normal file
View File

@ -0,0 +1,4 @@
function result = return_Point2Ptr(obj,value)
% usage: obj.return_Point2Ptr(value)
error('need to compile return_Point2Ptr.cpp');
end

12
wrap/tests/expected/@Test/return_Test.cpp Normal file
View File

@ -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");
}

4
wrap/tests/expected/@Test/return_Test.m Normal file
View File

@ -0,0 +1,4 @@
function result = return_Test(obj,value)
% usage: obj.return_Test(value)
error('need to compile return_Test.cpp');
end

5
wrap/tests/expected/@Test/return_TestPtr.cpp
View File

@ -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