diff --git a/.cproject b/.cproject
index 075558734..b31515394 100644
--- a/.cproject
+++ b/.cproject
@@ -308,6 +308,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -334,7 +342,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -342,7 +349,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -390,7 +396,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -398,7 +403,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -406,7 +410,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -422,20 +425,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j5
@@ -524,22 +518,6 @@
false
true
-
- make
- -j2
- tests/testPose2.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose3.run
- true
- true
- true
-
make
-j2
@@ -556,6 +534,22 @@
true
true
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
make
-j2
@@ -580,26 +574,26 @@
true
true
-
+
make
- -j2
- all
+ -j5
+ testValues.run
true
true
true
-
+
make
- -j2
- check
+ -j5
+ testOrdering.run
true
true
true
-
+
make
- -j2
- clean
+ -j5
+ testKey.run
true
true
true
@@ -684,26 +678,26 @@
true
true
-
+
make
- -j5
- testValues.run
+ -j2
+ all
true
true
true
-
+
make
- -j5
- testOrdering.run
+ -j2
+ check
true
true
true
-
+
make
- -j5
- testKey.run
+ -j2
+ clean
true
true
true
@@ -958,7 +952,6 @@
make
-
testGraph.run
true
false
@@ -966,7 +959,6 @@
make
-
testJunctionTree.run
true
false
@@ -974,7 +966,6 @@
make
-
testSymbolicBayesNetB.run
true
false
@@ -1110,7 +1101,6 @@
make
-
testErrors.run
true
false
@@ -1574,6 +1564,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -1613,6 +1604,7 @@
make
+
testSimulated2D.run
true
false
@@ -1620,6 +1612,7 @@
make
+
testSimulated3D.run
true
false
@@ -1835,6 +1828,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -1856,6 +1850,102 @@
true
true
+
+ make
+ -j2
+ testRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testRot2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testHomography2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPoint2.run
+ true
+ true
+ true
+
make
-j2
@@ -2057,7 +2147,6 @@
cpack
-
-G DEB
true
false
@@ -2065,7 +2154,6 @@
cpack
-
-G RPM
true
false
@@ -2073,7 +2161,6 @@
cpack
-
-G TGZ
true
false
@@ -2081,7 +2168,6 @@
cpack
-
--config CPackSourceConfig.cmake
true
false
@@ -2207,98 +2293,34 @@
true
true
-
+
make
- -j2
- testRot3.run
+ -j5
+ testSpirit.run
true
true
true
-
+
make
- -j2
- testRot2.run
+ -j5
+ testWrap.run
true
true
true
-
+
make
- -j2
- testPose3.run
+ -j5
+ check.wrap
true
true
true
-
+
make
- -j2
- timeRot3.run
- true
- true
- true
-
-
- make
- -j2
- testPose2.run
- true
- true
- true
-
-
- make
- -j2
- testCal3_S2.run
- true
- true
- true
-
-
- make
- -j2
- testSimpleCamera.run
- true
- true
- true
-
-
- make
- -j2
- testHomography2.run
- true
- true
- true
-
-
- make
- -j2
- testCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testPoint2.run
+ -j5
+ wrap
true
true
true
@@ -2342,46 +2364,7 @@
false
true
-
- make
- -j5
- testSpirit.run
- true
- true
- true
-
-
- make
- -j5
- testWrap.run
- true
- true
- true
-
-
- make
- -j5
- check.wrap
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam
- true
- true
- true
-
-
- make
- -j5
- wrap
- true
- true
- true
-
+
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 77b8b0c18..ed6b31db9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -36,41 +36,47 @@ else()
set(GTSAM_UNSTABLE_AVAILABLE 0)
endif()
+
# Configurable Options
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON)
option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON)
if(GTSAM_UNSTABLE_AVAILABLE)
- option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" OFF)
+ option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
endif()
-option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
-if(MSVC)
- option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" OFF)
-else()
+if(NOT MSVC)
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON)
-endif()
-option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON)
-option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF)
-if(MSVC)
- option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" OFF)
+ option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON)
else()
+ set(GTSAM_BUILD_STATIC_LIBRARY ON)
+endif()
+option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF)
+if(NOT MSVC)
option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" ON)
endif()
-option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON)
-option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON)
-option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON)
-option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON)
-set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox")
-set(GTSAM_WRAP_HEADER_PATH ${PROJECT_SOURCE_DIR}/wrap CACHE DOCSTRING "Path to directory of matlab.h")
+# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
+option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON)
+option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
+option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility for wrapping other libraries" ON)
+set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE DOCSTRING "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/toolbox")
+set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
+set(MEX_COMMAND "mex" CACHE FILEPATH "Command to use for executing mex (if on path, 'mex' will work)")
+
+# Check / set dependent variables for MATLAB wrapper
+set(GTSAM_WRAP_HEADER_PATH "${PROJECT_SOURCE_DIR}/wrap")
+if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT GTSAM_BUILD_WRAP)
+ message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
+endif()
+if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
+ message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
+endif()
+if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
+ set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/borg/toolbox")
+endif()
-# Flags for building/installing mex files
-option(GTSAM_BUILD_MEX_BIN "Enable/Disable building of matlab mex files" OFF)
-option(GTSAM_INSTALL_MEX_BIN "Enable/Disable installing matlab mex binaries" OFF)
-set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Flags for running Matlab MEX compilation")
-set(MEX_COMMAND "mex" CACHE STRING "Command to use for executing mex (if on path, 'mex' will work)")
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
@@ -90,7 +96,7 @@ endif(GTSAM_USE_QUATERNIONS)
# Flags to determine whether tests and examples are build during 'make install'
# Note that these remove the targets from the 'all'
option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON)
-option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables buildint examples during install" OFF)
+option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables building examples during install" OFF)
# Pull in infrastructure
if (GTSAM_BUILD_TESTS)
@@ -103,7 +109,8 @@ endif()
if(CYGWIN OR MSVC OR WIN32)
set(Boost_USE_STATIC_LIBS 1)
endif()
-find_package(Boost 1.43 COMPONENTS serialization REQUIRED)
+find_package(Boost 1.43 COMPONENTS serialization system filesystem thread date_time REQUIRED)
+set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY})
# General build settings
include_directories(
@@ -134,7 +141,9 @@ if (GTSAM_BUILD_EXAMPLES)
endif(GTSAM_BUILD_EXAMPLES)
# Matlab toolbox
-add_subdirectory(matlab)
+if (GTSAM_INSTALL_MATLAB_TOOLBOX)
+ add_subdirectory(matlab)
+endif()
# Build gtsam_unstable
if (GTSAM_BUILD_UNSTABLE)
@@ -168,11 +177,12 @@ message(STATUS "Build flags ")
print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ")
print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ")
print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
-print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
-print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ")
-print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ")
+if(NOT MSVC)
+ print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ")
+ print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ")
+ print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries ")
+endif()
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build-type in library name ")
-print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries ")
if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
endif()
@@ -192,11 +202,8 @@ print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default R
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")
-print_config_flag(${GTSAM_INSTALL_MATLAB_EXAMPLES} "Install matlab examples ")
-print_config_flag(${GTSAM_INSTALL_MATLAB_TESTS} "Install matlab tests ")
+print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ")
-print_config_flag(${GTSAM_BUILD_MEX_BIN} "Build MEX binaries ")
-print_config_flag(${GTSAM_INSTALL_MEX_BIN} "Install MEX binaries ")
message(STATUS "===============================================================")
# Include CPack *after* all flags
diff --git a/gtsam.h b/gtsam.h
index 48199cbf3..882319607 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -6,19 +6,26 @@
*
* Requirements:
* Classes must start with an uppercase letter
- * Only one Method/Constructor per line
+ * - Can wrap a typedef
+ * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines
* Methods can return
* - Eigen types: Matrix, Vector
* - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char
* - 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 (static or otherwise) with a given name
+ * Constructors
+ * - Overloads are supported
+ * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB
+ * Methods
* - Constness has no effect
- * Methods must start with a lowercase letter
- * Static methods must start with a letter (upper or lowercase) and use the "static" keyword
+ * - Specify by-value (not reference) return types, even if C++ method returns reference
+ * - Must start with a lowercase letter
+ * - Overloads are supported
+ * Static methods
+ * - Must start with a letter (upper or lowercase) and use the "static" keyword
+ * - The first letter will be made uppercase in the generated MATLAB interface
+ * - Overloads are supported
* Arguments to functions any of
* - Eigen types: Matrix, Vector
* - Eigen types and classes as an optionally const reference
@@ -35,7 +42,7 @@
* 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"
- * Using namespace
+ * Using namespace: FIXME: this functionality is currently broken
* - To use a namespace (e.g., generate a "using namespace x" line in cpp files), add "using namespace x;"
* - This declaration applies to all classes *after* the declaration, regardless of brackets
* Includes in C++ wrappers
@@ -44,17 +51,34 @@
* - To override, add a full include statement just before the class statement
* - An override include can be added for a namespace by placing it just before the namespace statement
* - Both classes and namespace accept exactly one namespace
- * Overriding type dependency checks
+ * Using classes defined in other modules
* - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error
- * - Limitation: this only works if the class does not need a namespace specification
+ * Virtual inheritance
+ * - Specify fully-qualified base classes, i.e. "virtual class Derived : module::Base {"
+ * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : module::Base {"
+ * - Forward declarations must also be marked virtual, e.g. "virtual class module::Base;" and
+ * also "virtual class module::Derived;"
+ * - Pure virtual (abstract) classes should list no constructors in this interface file
+ * - Virtual classes must have a clone() function in C++ (though it does not have to be included
+ * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead
+ * of using the copy constructor (which is used for non-virtual objects).
+ * Templates
+ * - Basic templates are supported either with an explicit list of types to instantiate,
+ * e.g. template class Class1 { ... };
+ * or with typedefs, e.g.
+ * template class Class2 { ... };
+ * typedef Class2 MyInstantiatedClass;
+ * - To create new instantiations in other modules, you must copy-and-paste the whole class definition
+ * into the new module, but use only your new instantiation types.
+ * - When forward-declaring template instantiations, use the generated/typedefed name, e.g.
+ * class gtsam::Class1Pose2;
+ * class gtsam::MyInstantiatedClass;
*/
/**
* Status:
* - TODO: global functions
* - TODO: default values for arguments
- * - TODO: overloaded functions
- * - TODO: signatures for constructors can be ambiguous if two types have the same first letter
* - TODO: Handle gtsam::Rot3M conversions to quaternions
*/
@@ -64,7 +88,17 @@ namespace gtsam {
// base
//*************************************************************************
-class LieVector {
+virtual class Value {
+ // No constructors because this is an abstract class
+
+ // Testable
+ void print(string s) const;
+
+ // Manifold
+ size_t dim() const;
+};
+
+virtual class LieVector : gtsam::Value {
// Standard constructors
LieVector();
LieVector(Vector v);
@@ -96,7 +130,7 @@ class LieVector {
// geometry
//*************************************************************************
-class Point2 {
+virtual class Point2 : gtsam::Value {
// Standard Constructors
Point2();
Point2(double x, double y);
@@ -128,7 +162,7 @@ class Point2 {
Vector vector() const;
};
-class StereoPoint2 {
+virtual class StereoPoint2 : gtsam::Value {
// Standard Constructors
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
@@ -157,7 +191,7 @@ class StereoPoint2 {
Vector vector() const;
};
-class Point3 {
+virtual class Point3 : gtsam::Value {
// Standard Constructors
Point3();
Point3(double x, double y, double z);
@@ -190,7 +224,7 @@ class Point3 {
double z() const;
};
-class Rot2 {
+virtual class Rot2 : gtsam::Value {
// Standard Constructors and Named Constructors
Rot2();
Rot2(double theta);
@@ -232,14 +266,14 @@ class Rot2 {
Matrix matrix() const;
};
-class Rot3 {
+virtual class Rot3 : gtsam::Value {
// Standard Constructors and Named Constructors
Rot3();
Rot3(Matrix R);
static gtsam::Rot3 Rx(double t);
static gtsam::Rot3 Ry(double t);
static gtsam::Rot3 Rz(double t);
-// static gtsam::Rot3 RzRyRx(double x, double y, double z); // FIXME: overloaded functions don't work yet
+ static gtsam::Rot3 RzRyRx(double x, double y, double z); // FIXME: overloaded functions don't work yet
static gtsam::Rot3 RzRyRx(Vector xyz);
static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading)
static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
@@ -261,7 +295,7 @@ class Rot3 {
// Manifold
static size_t Dim();
size_t dim() const;
- // gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
+ gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const;
@@ -284,7 +318,7 @@ class Rot3 {
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
};
-class Pose2 {
+virtual class Pose2 : gtsam::Value {
// Standard Constructor
Pose2();
Pose2(double x, double y, double theta);
@@ -330,12 +364,12 @@ class Pose2 {
Matrix matrix() const;
};
-class Pose3 {
+virtual class Pose3 : gtsam::Value {
// Standard Constructors
Pose3();
Pose3(const gtsam::Pose3& pose);
Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
- // Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose)
+ Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose)
Pose3(Matrix t);
// Testable
@@ -373,19 +407,20 @@ class Pose3 {
double y() const;
double z() const;
Matrix matrix() const;
- // gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
+ gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
double range(const gtsam::Point3& point);
- // double range(const gtsam::Pose3& pose); // FIXME: shadows other range
+ double range(const gtsam::Pose3& pose);
};
-class Cal3_S2 {
+virtual class Cal3_S2 : gtsam::Value {
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
+ Cal3_S2(Vector v);
// Testable
void print(string s) const;
- bool equals(const gtsam::Cal3_S2& pose, double tol) const;
+ bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
// Manifold
static size_t Dim();
@@ -409,6 +444,37 @@ class Cal3_S2 {
Matrix matrix_inverse() const;
};
+virtual class Cal3DS2 : gtsam::Value {
+ // Standard Constructors
+ Cal3DS2();
+ Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
+ Cal3DS2(Vector v);
+
+ // Testable
+ void print(string s) const;
+ bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
+
+ // Manifold
+ static size_t Dim();
+ size_t dim() const;
+ gtsam::Cal3DS2 retract(Vector v) const;
+ Vector localCoordinates(const gtsam::Cal3DS2& c) const;
+
+ // Action on Point2
+ gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+ // TODO: D2d functions that start with an uppercase letter
+
+ // Standard Interface
+ double fx() const;
+ double fy() const;
+ double skew() const;
+ double px() const;
+ double py() const;
+ Vector vector() const;
+ Vector k() const;
+ //Matrix K() const; //FIXME: Uppercase
+};
+
class Cal3_S2Stereo {
// Standard Constructors
Cal3_S2Stereo();
@@ -428,7 +494,7 @@ class Cal3_S2Stereo {
double baseline() const;
};
-class CalibratedCamera {
+virtual class CalibratedCamera : gtsam::Value {
// Standard Constructors and Named Constructors
CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose);
@@ -458,14 +524,14 @@ class CalibratedCamera {
double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
};
-class SimpleCamera {
+virtual class SimpleCamera : gtsam::Value {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
static gtsam::SimpleCamera level(const gtsam::Cal3_S2& K,
const gtsam::Pose2& pose, double height);
- // static gtsam::SimpleCamera level(const gtsam::Pose2& pose, double height); // FIXME overload
+ static gtsam::SimpleCamera level(const gtsam::Pose2& pose, double height); // FIXME overload
static gtsam::SimpleCamera lookat(const gtsam::Point3& eye,
const gtsam::Point3& target, const gtsam::Point3& upVector,
const gtsam::Cal3_S2& K);
@@ -490,7 +556,7 @@ class SimpleCamera {
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
double range(const gtsam::Point3& point);
- // double range(const gtsam::Pose3& point); // FIXME, overload
+ double range(const gtsam::Pose3& point); // FIXME, overload
};
//*************************************************************************
@@ -679,17 +745,17 @@ class VariableIndex {
#include
namespace noiseModel {
-class Base {
+virtual class Base {
};
-class Gaussian {
+virtual class Gaussian : gtsam::noiseModel::Base {
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
// Matrix R() const; // FIXME: cannot parse!!!
void print(string s) const;
};
-class Diagonal {
+virtual class Diagonal : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
static gtsam::noiseModel::Diagonal* Variances(Vector variances);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
@@ -697,14 +763,14 @@ class Diagonal {
void print(string s) const;
};
-class Isotropic {
+virtual class Isotropic : gtsam::noiseModel::Diagonal {
static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
void print(string s) const;
};
-class Unit {
+virtual class Unit : gtsam::noiseModel::Isotropic {
static gtsam::noiseModel::Unit* Create(size_t dim);
void print(string s) const;
};
@@ -759,7 +825,7 @@ class GaussianBayesNet {
void push_front(gtsam::GaussianConditional* conditional);
};
-class GaussianFactor {
+virtual class GaussianFactor {
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
@@ -767,7 +833,7 @@ class GaussianFactor {
size_t size() const;
};
-class JacobianFactor {
+virtual class JacobianFactor : gtsam::GaussianFactor {
JacobianFactor();
JacobianFactor(Vector b_in);
JacobianFactor(size_t i1, Matrix A1, Vector b,
@@ -787,7 +853,7 @@ class JacobianFactor {
gtsam::GaussianFactor* negate() const;
};
-class HessianFactor {
+virtual class HessianFactor : gtsam::GaussianFactor {
HessianFactor(const gtsam::HessianFactor& gf);
HessianFactor();
HessianFactor(size_t j, Matrix G, Vector g, double f);
@@ -811,22 +877,19 @@ class GaussianFactorGraph {
GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN);
// From FactorGraph
- void push_back(gtsam::GaussianFactor* factor);
void print(string s) const;
bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
size_t size() const;
gtsam::GaussianFactor* at(size_t idx) const;
// Building the graph
- void add(gtsam::JacobianFactor* factor);
- // all these won't work as MATLAB can't handle overloading
-// void add(Vector b);
-// void add(size_t key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
-// void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b,
-// const gtsam::SharedDiagonal& model);
-// void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3,
-// Vector b, const gtsam::SharedDiagonal& model);
-// void add(gtsam::HessianFactor* factor);
+ void push_back(gtsam::GaussianFactor* factor);
+ void add(Vector b);
+ void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
+ void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b,
+ const gtsam::noiseModel::Diagonal* model);
+ void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3,
+ Vector b, const gtsam::noiseModel::Diagonal* model);
// error and probability
double error(const gtsam::VectorValues& c) const;
@@ -909,29 +972,55 @@ class Ordering {
void insert(size_t key, size_t order);
void push_back(size_t key);
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
- // FIXME: Wrap InvertedMap as well
- //InvertedMap invert() const;
+ gtsam::InvertedOrdering invert() const;
+};
+
+#include
+class InvertedOrdering {
+ InvertedOrdering();
+
+ // FIXME: add bracket operator overload
+
+ bool empty() const;
+ size_t size() const;
+ bool count(size_t index) const; // Use as a boolean function with implicit cast
+
+ void clear();
};
class NonlinearFactorGraph {
NonlinearFactorGraph();
void print(string s) const;
+ double error(const gtsam::Values& c) const;
+ double probPrime(const gtsam::Values& c) const;
+ gtsam::NonlinearFactor* at(size_t i) const;
+ void add(const gtsam::NonlinearFactor* factor);
+ gtsam::Ordering* orderingCOLAMD(const gtsam::Values& c) const;
+ // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const;
+ gtsam::GaussianFactorGraph* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const;
+ gtsam::NonlinearFactorGraph clone() const;
};
-class NonlinearFactor {
-// NonlinearFactor(); // FIXME: don't use this - creates an abstract class
+virtual class NonlinearFactor {
void print(string s) const;
void equals(const gtsam::NonlinearFactor& other, double tol) const;
gtsam::KeyVector keys() const;
size_t size() const;
- size_t dim() const; // FIXME: Doesn't link
+ size_t dim() const;
+ double error(const gtsam::Values& c) const;
+ bool active(const gtsam::Values& c) const;
+ gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const;
+ gtsam::NonlinearFactor* clone() const;
+ // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen
};
class Values {
Values();
size_t size() const;
void print(string s) const;
+ void insert(size_t j, const gtsam::Value& value);
bool exists(size_t j) const;
+ gtsam::Value at(size_t j) const;
};
// Actually a FastList
@@ -1012,13 +1101,13 @@ class LevenbergMarquardtParams {
LevenbergMarquardtParams();
void print(string s) const;
- double getMaxIterations() const;
+ size_t getMaxIterations() const;
double getRelativeErrorTol() const;
double getAbsoluteErrorTol() const;
double getErrorTol() const;
string getVerbosity() const;
- void setMaxIterations(double value);
+ void setMaxIterations(size_t value);
void setRelativeErrorTol(double value);
void setAbsoluteErrorTol(double value);
void setErrorTol(double value);
@@ -1040,6 +1129,59 @@ class LevenbergMarquardtParams {
void setVerbosityLM(string s);
};
+//*************************************************************************
+// Nonlinear factor types
+//*************************************************************************
+template
+virtual class PriorFactor : gtsam::NonlinearFactor {
+ PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
+};
+
+
+template
+virtual class BetweenFactor : gtsam::NonlinearFactor {
+ BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
+};
+
+
+template
+virtual class NonlinearEquality : gtsam::NonlinearFactor {
+ // Constructor - forces exact evaluation
+ NonlinearEquality(size_t j, const T& feasible);
+ // Constructor - allows inexact evaluation
+ NonlinearEquality(size_t j, const T& feasible, double error_gain);
+};
+
+
+template
+virtual class RangeFactor : gtsam::NonlinearFactor {
+ RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
+};
+
+typedef gtsam::RangeFactor RangeFactor2D;
+typedef gtsam::RangeFactor RangeFactor3D;
+typedef gtsam::RangeFactor RangeFactorCalibratedCamera;
+typedef gtsam::RangeFactor RangeFactorSimpleCamera;
+
+template
+virtual class BearingFactor : gtsam::NonlinearFactor {
+ BearingFactor(size_t key1, size_t key2, const ROT& measured, const gtsam::noiseModel::Base* noiseModel);
+};
+
+typedef gtsam::BearingFactor BearingFactor2D;
+
+
+#include
+template
+virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
+ GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
+ size_t poseKey, size_t pointKey, const CALIBRATION* k);
+ gtsam::Point2 measured() const;
+ CALIBRATION* calibration() const;
+};
+typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2;
+typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2;
+
}///\namespace gtsam
//*************************************************************************
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index fb732ca51..66870a09c 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -95,9 +95,6 @@ if (GTSAM_BUILD_STATIC_LIBRARY)
list(APPEND GTSAM_EXPORTED_TARGETS gtsam-static)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
set(gtsam-lib gtsam-static)
- if(NOT GTSAM_BUILD_SHARED_LIBRARY)
- set(gtsam-prefer-shared gtsam-static)
- endif()
endif (GTSAM_BUILD_STATIC_LIBRARY)
if (GTSAM_BUILD_SHARED_LIBRARY)
@@ -115,34 +112,23 @@ if (GTSAM_BUILD_SHARED_LIBRARY)
if (NOT GTSAM_BUILD_STATIC_LIBRARY)
set(gtsam-lib "gtsam-shared")
endif()
- set(gtsam-prefer-shared gtsam-shared)
endif(GTSAM_BUILD_SHARED_LIBRARY)
# Create the matlab toolbox for the gtsam library
-if (GTSAM_BUILD_WRAP)
+if (GTSAM_INSTALL_MATLAB_TOOLBOX)
# Set up codegen
include(GtsamMatlabWrap)
# TODO: generate these includes programmatically
# Choose include flags depending on build process
- if (GTSAM_BUILD_MEX_BIN)
- set(MEX_INCLUDE_ROOT ${GTSAM_SOURCE_ROOT_DIR})
- set(MEX_LIB_ROOT ${CMAKE_BINARY_DIR})
- set(GTSAM_LIB_DIR ${MEX_LIB_ROOT}/gtsam)
- else()
- set(MEX_INCLUDE_ROOT ${CMAKE_INSTALL_PREFIX}/include)
- set(MEX_LIB_ROOT ${CMAKE_INSTALL_PREFIX}/lib)
- set(GTSAM_LIB_DIR ${MEX_LIB_ROOT})
- endif()
+ set(MEX_INCLUDE_ROOT ${GTSAM_SOURCE_ROOT_DIR})
+ set(MEX_LIB_ROOT ${CMAKE_BINARY_DIR})
+ set(GTSAM_LIB_DIR ${MEX_LIB_ROOT}/gtsam)
# Generate, build and install toolbox
- string(TOUPPER ${CMAKE_BUILD_TYPE} build_type_toupper)
- get_target_property(gtsam_library_file ${gtsam-prefer-shared} LOCATION_${build_type_toupper})
- set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${MEX_INCLUDE_ROOT}/gtsam -I${MEX_INCLUDE_ROOT}/gtsam/base -I${MEX_INCLUDE_ROOT}/gtsam/geometry -I${MEX_INCLUDE_ROOT}/gtsam/linear -I${MEX_INCLUDE_ROOT}/gtsam/discrete -I${MEX_INCLUDE_ROOT}/gtsam/inference -I${MEX_INCLUDE_ROOT}/gtsam/nonlinear -I${MEX_INCLUDE_ROOT}/gtsam/slam ${gtsam_library_file}")
- # Lots of escapes '\' here because they get eaten during subsequent calls to 'set'
- set(mexFlags "${mexFlags} -g COMPFLAGS=\\\"$$COMPFLAGS ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${build_type_toupper}}\\\"")
+ set(mexFlags ${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${MEX_INCLUDE_ROOT}/gtsam -I${MEX_INCLUDE_ROOT}/gtsam/base -I${MEX_INCLUDE_ROOT}/gtsam/geometry -I${MEX_INCLUDE_ROOT}/gtsam/linear -I${MEX_INCLUDE_ROOT}/gtsam/discrete -I${MEX_INCLUDE_ROOT}/gtsam/inference -I${MEX_INCLUDE_ROOT}/gtsam/nonlinear -I${MEX_INCLUDE_ROOT}/gtsam/slam)
# Macro to handle details of setting up targets
# FIXME: issue with dependency between wrap_gtsam and wrap_gtsam_build, only shows up on CMake 2.8.3
- wrap_library(gtsam "${mexFlags}" "../")
+ wrap_library(gtsam "${mexFlags}" "../" "")
endif ()
diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index 6c10a29f5..4f6dae9c7 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -16,7 +16,7 @@
*/
#pragma once
-
+#include
#include
#include
@@ -37,7 +37,8 @@ public:
/**
* Create a duplicate object returned as a pointer to the generic Value interface.
- * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator
+ * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
+ * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
*/
virtual Value* clone_() const {
void *place = boost::singleton_pool::malloc();
@@ -53,6 +54,13 @@ public:
boost::singleton_pool::free((void*)this);
}
+ /**
+ * Clone this value (normal clone on the heap, delete with 'delete' operator)
+ */
+ virtual boost::shared_ptr clone() const {
+ return boost::make_shared(static_cast(*this));
+ }
+
/// equals implementing generic Value interface
virtual bool equals_(const Value& p, double tol = 1e-9) const {
// Cast the base class Value pointer to a derived class pointer
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 394a11e72..8b19a6532 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -101,12 +101,15 @@ namespace gtsam {
class Value {
public:
- /** Allocate and construct a clone of this value */
+ /** Clone this value in a special memory pool, must be deleted with Value::deallocate_, *not* with the 'delete' operator. */
virtual Value* clone_() const = 0;
/** Deallocate a raw pointer of this value */
virtual void deallocate_() const = 0;
+ /** Clone this value (normal clone on the heap, delete with 'delete' operator) */
+ virtual boost::shared_ptr clone() const = 0;
+
/** Compare this Value with another for equality. */
virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index b4878565e..fd1ec1f6c 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -83,32 +83,32 @@ namespace gtsam {
push_back(fg);
}
- /** Add a Jacobian factor */
- void add(const boost::shared_ptr& factor) {
- factors_.push_back(boost::shared_ptr(factor));
+ /** Add a factor by value - makes a copy */
+ void add(const GaussianFactor& factor) {
+ factors_.push_back(factor.clone());
}
- /** Add a Hessian factor */
- void add(const boost::shared_ptr& factor) {
- factors_.push_back(boost::shared_ptr(factor));
+ /** Add a factor by pointer - stores pointer without copying the factor */
+ void add(const sharedFactor& factor) {
+ factors_.push_back(factor);
}
/** Add a null factor */
void add(const Vector& b) {
- add(JacobianFactor::shared_ptr(new JacobianFactor(b)));
+ add(JacobianFactor(b));
}
/** Add a unary factor */
void add(Index key1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) {
- add(JacobianFactor::shared_ptr(new JacobianFactor(key1,A1,b,model)));
+ add(JacobianFactor(key1,A1,b,model));
}
/** Add a binary factor */
void add(Index key1, const Matrix& A1,
Index key2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) {
- add(JacobianFactor::shared_ptr(new JacobianFactor(key1,A1,key2,A2,b,model)));
+ add(JacobianFactor(key1,A1,key2,A2,b,model));
}
/** Add a ternary factor */
@@ -116,13 +116,13 @@ namespace gtsam {
Index key2, const Matrix& A2,
Index key3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model) {
- add(JacobianFactor::shared_ptr(new JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)));
+ add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model));
}
/** Add an n-ary factor */
void add(const std::vector > &terms,
const Vector &b, const SharedDiagonal& model) {
- add(JacobianFactor::shared_ptr(new JacobianFactor(terms,b,model)));
+ add(JacobianFactor(terms,b,model));
}
/**
diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h
index aeec47a39..df4cd1d65 100644
--- a/gtsam/linear/KalmanFilter.h
+++ b/gtsam/linear/KalmanFilter.h
@@ -17,6 +17,8 @@
* @author Frank Dellaert
*/
+#pragma once
+
#include
#include
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index aa9c1aa6d..9fad99223 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -419,7 +419,7 @@ namespace gtsam {
maps_.reserve(maps_.size() + dimensions.size());
BOOST_FOREACH(size_t dim, dimensions) {
maps_.push_back(values_.segment(varStart, dim));
- varStart += dim; // varStart is continued from first for loop
+ varStart += (int)dim; // varStart is continued from first for loop
}
}
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 85cc0e63f..f7ebb42bc 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -55,9 +55,14 @@ namespace gtsam {
/** Unnormalized probability. O(n) */
double probPrime(const Values& c) const;
- template
- void add(const F& factor) {
- this->push_back(boost::shared_ptr(new F(factor)));
+ /// Add a factor by value - copies the factor object
+ void add(const NonlinearFactor& factor) {
+ this->push_back(factor.clone());
+ }
+
+ /// Add a factor by pointer - stores pointer without copying factor object
+ void add(const sharedFactor& factor) {
+ this->push_back(factor);
}
/**
diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h
index 44b352938..1c071faef 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.h
+++ b/gtsam/nonlinear/NonlinearOptimizer.h
@@ -45,23 +45,23 @@ public:
Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT)
NonlinearOptimizerParams() :
- maxIterations(100.0), relativeErrorTol(1e-5), absoluteErrorTol(1e-5),
+ maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5),
errorTol(0.0), verbosity(SILENT) {}
virtual ~NonlinearOptimizerParams() {}
virtual void print(const std::string& str = "") const ;
- inline double getMaxIterations() const { return maxIterations; }
- inline double getRelativeErrorTol() const { return relativeErrorTol; }
- inline double getAbsoluteErrorTol() const { return absoluteErrorTol; }
- inline double getErrorTol() const { return errorTol; }
- inline std::string getVerbosity() const { return verbosityTranslator(verbosity); }
+ size_t getMaxIterations() const { return maxIterations; }
+ double getRelativeErrorTol() const { return relativeErrorTol; }
+ double getAbsoluteErrorTol() const { return absoluteErrorTol; }
+ double getErrorTol() const { return errorTol; }
+ std::string getVerbosity() const { return verbosityTranslator(verbosity); }
- inline void setMaxIterations(double value) { maxIterations = value; }
- inline void setRelativeErrorTol(double value) { relativeErrorTol = value; }
- inline void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
- inline void setErrorTol(double value) { errorTol = value ; }
- inline void setVerbosity(const std::string &src) { verbosity = verbosityTranslator(src); }
+ void setMaxIterations(size_t value) { maxIterations = value; }
+ void setRelativeErrorTol(double value) { relativeErrorTol = value; }
+ void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
+ void setErrorTol(double value) { errorTol = value ; }
+ void setVerbosity(const std::string &src) { verbosity = verbosityTranslator(src); }
Verbosity verbosityTranslator(const std::string &s) const;
std::string verbosityTranslator(Verbosity value) const;
diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h
index 098976a36..c6881fbde 100644
--- a/gtsam/nonlinear/Ordering.h
+++ b/gtsam/nonlinear/Ordering.h
@@ -155,7 +155,7 @@ public:
iterator end() { return order_.end(); }
/// Test if the key exists in the ordering.
- bool exists(Key key) const { return order_.count(key); }
+ bool exists(Key key) const { return order_.count(key) > 0; }
///TODO: comment
std::pair tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); }
@@ -233,7 +233,10 @@ private:
ar & BOOST_SERIALIZATION_NVP(order_);
ar & BOOST_SERIALIZATION_NVP(nVars_);
}
-};
+}; // \class Ordering
+
+// typedef for use with matlab
+typedef Ordering::InvertedMap InvertedOrdering;
/**
* @class Unordered
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index c42c2798d..62e46d53e 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -25,12 +25,12 @@ namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
- template
+ template
class BearingFactor: public NoiseModelFactor2 {
private:
typedef POSE Pose;
- typedef typename Pose::Rotation Rot;
+ typedef ROT Rot;
typedef POINT Point;
typedef BearingFactor This;
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index bce7bcf84..048756435 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -62,7 +62,7 @@ namespace gtsam {
* @param K shared pointer to the constant calibration
*/
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
- const Key poseKey, Key pointKey, const shared_ptrK& K) :
+ Key poseKey, Key pointKey, const boost::shared_ptr& K) :
Base(model, poseKey, pointKey), measured_(measured), K_(K) {
}
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index acd36ddc2..09fb124a9 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -15,10 +15,12 @@ add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-fail
foreach(subdir ${gtsam_unstable_subdirs})
# Build convenience libraries
file(GLOB subdir_srcs "${subdir}/*.cpp")
- set(${subdir}_srcs ${subdir_srcs})
+ file(GLOB subdir_headers "${subdir}/*.h")
+ set(${subdir}_srcs ${subdir_srcs} ${subdir_headers})
+ gtsam_assign_source_folders("${${subdir}_srcs}") # Create MSVC structure
if (subdir_srcs AND GTSAM_BUILD_CONVENIENCE_LIBRARIES)
message(STATUS "Building Convenience Library: ${subdir}_unstable")
- add_library("${subdir}_unstable" STATIC ${subdir_srcs})
+ add_library("${subdir}_unstable" STATIC ${${subdir}_srcs})
endif()
# Build local library and tests
@@ -36,7 +38,9 @@ set(gtsam_unstable_srcs
${slam_srcs}
)
-option (GTSAM_UNSTABLE_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam_unstable" ON)
+if(NOT MSVC)
+ option (GTSAM_UNSTABLE_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam_unstable" ON)
+endif()
# Versions - same as core gtsam library
set(gtsam_unstable_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH})
@@ -73,28 +77,21 @@ if (GTSAM_UNSTABLE_BUILD_SHARED_LIBRARY)
endif(GTSAM_UNSTABLE_BUILD_SHARED_LIBRARY)
# Wrap version for gtsam_unstable
-if (GTSAM_BUILD_WRAP)
+if (GTSAM_INSTALL_MATLAB_TOOLBOX)
# Set up codegen
include(GtsamMatlabWrap)
# TODO: generate these includes programmatically
# Choose include flags depending on build process
- if (GTSAM_BUILD_MEX_BIN)
- set(MEX_INCLUDE_ROOT ${GTSAM_SOURCE_ROOT_DIR})
- set(MEX_LIB_ROOT ${CMAKE_BINARY_DIR})
- set(GTSAM_LIB_DIR ${MEX_LIB_ROOT}/gtsam)
- set(GTSAM_UNSTABLE_LIB_DIR ${MEX_LIB_ROOT}/gtsam_unstable)
- else()
- set(MEX_INCLUDE_ROOT ${CMAKE_INSTALL_PREFIX}/include)
- set(MEX_LIB_ROOT ${CMAKE_INSTALL_PREFIX}/lib)
- set(GTSAM_LIB_DIR ${MEX_LIB_ROOT})
- set(GTSAM_UNSTABLE_LIB_DIR ${MEX_LIB_ROOT})
- endif()
+ set(MEX_INCLUDE_ROOT ${GTSAM_SOURCE_ROOT_DIR})
+ set(MEX_LIB_ROOT ${CMAKE_BINARY_DIR})
+ set(GTSAM_LIB_DIR ${MEX_LIB_ROOT}/gtsam)
+ set(GTSAM_UNSTABLE_LIB_DIR ${MEX_LIB_ROOT}/gtsam_unstable)
# Generate, build and install toolbox
- set(mexFlags "-I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${MEX_INCLUDE_ROOT}/gtsam_unstable -I${MEX_INCLUDE_ROOT}/gtsam_unstable/dynamics -I${MEX_INCLUDE_ROOT}/gtsam_unstable/discrete -L${GTSAM_UNSTABLE_LIB_DIR} -L${GTSAM_LIB_DIR} -lgtsam -lgtsam_unstable")
+ set(mexFlags -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${MEX_INCLUDE_ROOT}/gtsam_unstable -I${MEX_INCLUDE_ROOT}/gtsam_unstable/dynamics -I${MEX_INCLUDE_ROOT}/gtsam_unstable/discrete)
# Macro to handle details of setting up targets
- wrap_library(gtsam_unstable "${mexFlags}" "./")
+ wrap_library(gtsam_unstable "${mexFlags}" "./" gtsam)
-endif(GTSAM_BUILD_WRAP)
+endif(GTSAM_INSTALL_MATLAB_TOOLBOX)
diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h
index c9a75ec23..f42ecc352 100644
--- a/gtsam_unstable/dynamics/FullIMUFactor.h
+++ b/gtsam_unstable/dynamics/FullIMUFactor.h
@@ -77,8 +77,6 @@ public:
const Vector& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); }
- const Key& key1() const { return this->key1_; }
- const Key& key2() const { return this->key2_; }
/**
* Error evaluation with optional derivatives - calculates
diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h
index adcf772e3..9e5651cc9 100644
--- a/gtsam_unstable/dynamics/IMUFactor.h
+++ b/gtsam_unstable/dynamics/IMUFactor.h
@@ -70,8 +70,6 @@ public:
const Vector& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); }
- const Key& key1() const { return this->key1_; }
- const Key& key2() const { return this->key2_; }
/**
* Error evaluation with optional derivatives - calculates
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index c9d114d58..3d9fd7aac 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -3,10 +3,12 @@
*/
// specify the classes from gtsam we are using
-class gtsam::Point3;
-class gtsam::Rot3;
-class gtsam::Pose3;
-class gtsam::SharedNoiseModel;
+virtual class gtsam::Value;
+virtual class gtsam::Point3;
+virtual class gtsam::Rot3;
+virtual class gtsam::Pose3;
+virtual class gtsam::noiseModel::Base;
+virtual class gtsam::NonlinearFactor;
namespace gtsam {
@@ -18,7 +20,7 @@ class Dummy {
};
#include
-class PoseRTV {
+virtual class PoseRTV : gtsam::Value {
PoseRTV();
PoseRTV(Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
@@ -66,6 +68,107 @@ class PoseRTV {
Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
};
+
+// Nonlinear factors from gtsam, for our Value types
+#include
+template
+virtual class PriorFactor : gtsam::NonlinearFactor {
+ PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
+};
+
+
+#include
+template
+virtual class BetweenFactor : gtsam::NonlinearFactor {
+ BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
+};
+
+
+#include
+template
+virtual class RangeFactor : gtsam::NonlinearFactor {
+ RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
+};
+
+typedef gtsam::RangeFactor RangeFactorRTV;
+
+
+#include
+template
+virtual class NonlinearEquality : gtsam::NonlinearFactor {
+ // Constructor - forces exact evaluation
+ NonlinearEquality(size_t j, const T& feasible);
+ // Constructor - allows inexact evaluation
+ NonlinearEquality(size_t j, const T& feasible, double error_gain);
+};
+
+
+template
+virtual class IMUFactor : gtsam::NonlinearFactor {
+ /** Standard constructor */
+ IMUFactor(Vector accel, Vector gyro,
+ double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
+
+ /** Full IMU vector specification */
+ IMUFactor(Vector imu_vector,
+ double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
+
+ Vector gyro() const;
+ Vector accel() const;
+ Vector z() const;
+ size_t key1() const;
+ size_t key2() const;
+};
+
+
+template
+virtual class FullIMUFactor : gtsam::NonlinearFactor {
+ /** Standard constructor */
+ FullIMUFactor(Vector accel, Vector gyro,
+ double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
+
+ /** Single IMU vector - imu = [accel, gyro] */
+ FullIMUFactor(Vector imu,
+ double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
+
+ Vector gyro() const;
+ Vector accel() const;
+ Vector z() const;
+ size_t key1() const;
+ size_t key2() const;
+};
+
+
+#include
+virtual class DHeightPrior : gtsam::NonlinearFactor {
+ DHeightPrior(size_t key, double height, const gtsam::noiseModel::Base* model);
+};
+
+
+#include
+virtual class DRollPrior : gtsam::NonlinearFactor {
+ /** allows for explicit roll parameterization - uses canonical coordinate */
+ DRollPrior(size_t key, double wx, const gtsam::noiseModel::Base* model);
+ /** Forces roll to zero */
+ DRollPrior(size_t key, const gtsam::noiseModel::Base* model);
+};
+
+
+#include
+virtual class VelocityPrior : gtsam::NonlinearFactor {
+ VelocityPrior(size_t key, Vector vel, const gtsam::noiseModel::Base* model);
+};
+
+
+#include
+virtual class DGroundConstraint : gtsam::NonlinearFactor {
+ // Primary constructor allows for variable height of the "floor"
+ DGroundConstraint(size_t key, double height, const gtsam::noiseModel::Base* model);
+ // Fully specify vector - use only for debugging
+ DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
+};
+
+
}///\namespace gtsam
#include
@@ -84,18 +187,18 @@ class Graph {
void print(string s) const;
// prior factors
- void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel);
+ void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::noiseModel::Base* noiseModel);
void addConstraint(size_t key, const gtsam::PoseRTV& pose);
- void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel);
+ void addHeightPrior(size_t key, double z, const gtsam::noiseModel::Base* noiseModel);
// inertial factors
- void addFullIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel);
- void addIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel);
- void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel);
+ void addFullIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::noiseModel::Base* noiseModel);
+ void addIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::noiseModel::Base* noiseModel);
+ void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::noiseModel::Base* noiseModel);
// other measurements
- void addBetween(size_t key1, size_t key2, const gtsam::PoseRTV& z, const gtsam::SharedNoiseModel& noiseModel);
- void addRange(size_t key1, size_t key2, double z, const gtsam::SharedNoiseModel& noiseModel);
+ void addBetween(size_t key1, size_t key2, const gtsam::PoseRTV& z, const gtsam::noiseModel::Base* noiseModel);
+ void addRange(size_t key1, size_t key2, double z, const gtsam::noiseModel::Base* noiseModel);
// optimization
imu::Values optimize(const imu::Values& init) const;
diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt
index 9d48a949f..84df4055c 100644
--- a/matlab/CMakeLists.txt
+++ b/matlab/CMakeLists.txt
@@ -1,37 +1,29 @@
# Install matlab components
-if (GTSAM_BUILD_WRAP)
- if (GTSAM_INSTALL_MATLAB_TOOLBOX)
- # Utility functions
- message(STATUS "Installing Matlab Utility Functions")
- # Matlab files: *.m and *.fig
- file(GLOB matlab_utils_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.m")
- file(GLOB matlab_utils_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.fig")
- set(matlab_utils ${matlab_utils_m} ${matlab_utils_fig})
- install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam)
-
- # Tests
- if (GTSAM_INSTALL_MATLAB_TESTS)
- message(STATUS "Installing Matlab Toolbox Tests")
- file(GLOB matlab_tests "${GTSAM_SOURCE_ROOT_DIR}/matlab/tests/*.m")
- install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/tests)
- endif ()
-
- # Examples
- if (GTSAM_INSTALL_MATLAB_EXAMPLES)
- message(STATUS "Installing Matlab Toolbox Examples")
- # Matlab files: *.m and *.fig
- file(GLOB matlab_examples_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.m")
- file(GLOB matlab_examples_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.fig")
- set(matlab_examples ${matlab_examples_m} ${matlab_examples_fig})
- install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/examples)
-
- message(STATUS "Installing Matlab Toolbox Examples (Data)")
- # Data files: *.graph and *.txt
- file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
- file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
- set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_txt})
- install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/Data)
- endif ()
- endif ()
-endif ()
+# Utility functions
+message(STATUS "Installing Matlab Utility Functions")
+# Matlab files: *.m and *.fig
+file(GLOB matlab_utils_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.m")
+file(GLOB matlab_utils_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.fig")
+set(matlab_utils ${matlab_utils_m} ${matlab_utils_fig})
+install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam)
+
+# Tests
+message(STATUS "Installing Matlab Toolbox Tests")
+file(GLOB matlab_tests "${GTSAM_SOURCE_ROOT_DIR}/matlab/tests/*.m")
+install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/tests)
+
+# Examples
+message(STATUS "Installing Matlab Toolbox Examples")
+# Matlab files: *.m and *.fig
+file(GLOB matlab_examples_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.m")
+file(GLOB matlab_examples_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.fig")
+set(matlab_examples ${matlab_examples_m} ${matlab_examples_fig})
+install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/examples)
+
+message(STATUS "Installing Matlab Toolbox Examples (Data)")
+# Data files: *.graph and *.txt
+file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
+file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
+set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_txt})
+install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/Data)
diff --git a/matlab/VisualISAMGenerateData.m b/matlab/VisualISAMGenerateData.m
index 3c55417d9..1c71185e2 100644
--- a/matlab/VisualISAMGenerateData.m
+++ b/matlab/VisualISAMGenerateData.m
@@ -29,7 +29,7 @@ data.K = truth.K;
for i=1:options.nrCameras
theta = (i-1)*2*pi/options.nrCameras;
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
- truth.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K);
+ truth.cameras{i} = gtsamSimpleCamera.Lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K);
% Create measurements
for j=1:nrPoints
% All landmarks seen in every frame
diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m
index 139d40e65..b2b960a7b 100644
--- a/matlab/VisualISAMInitialize.m
+++ b/matlab/VisualISAMInitialize.m
@@ -6,10 +6,10 @@ function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options)
isam = visualSLAMISAM(options.reorderInterval);
%% Set Noise parameters
-noiseModels.pose = gtsamnoiseModelDiagonal_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-noiseModels.odometry = gtsamnoiseModelDiagonal_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-noiseModels.point = gtsamnoiseModelIsotropic_Sigma(3, 0.1);
-noiseModels.measurement = gtsamnoiseModelIsotropic_Sigma(2, 1.0);
+noiseModels.pose = gtsamnoiseModelDiagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+noiseModels.odometry = gtsamnoiseModelDiagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+noiseModels.point = gtsamnoiseModelIsotropic.Sigma(3, 0.1);
+noiseModels.measurement = gtsamnoiseModelIsotropic.Sigma(2, 1.0);
%% Add constraints/priors
% TODO: should not be from ground truth!
diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m
index d3e563dd4..3f48bb8e4 100644
--- a/matlab/examples/LocalizationExample.m
+++ b/matlab/examples/LocalizationExample.m
@@ -20,13 +20,13 @@ graph = pose2SLAMGraph;
%% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
%% Add three "GPS" measurements
% We use Pose2 Priors here with high variance on theta
-noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]);
+noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.1; 10]);
graph.addPosePrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel);
graph.addPosePrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel);
graph.addPosePrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel);
diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m
index b3cbff6a2..e90550152 100644
--- a/matlab/examples/OdometryExample.m
+++ b/matlab/examples/OdometryExample.m
@@ -20,12 +20,12 @@ graph = pose2SLAMGraph;
%% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
-priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
%% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m
index 499845eda..7557bd0ba 100644
--- a/matlab/examples/PlanarSLAMExample.m
+++ b/matlab/examples/PlanarSLAMExample.m
@@ -28,18 +28,18 @@ graph = planarSLAMGraph;
%% Add prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
+priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
%% Add odometry
odometry = gtsamPose2(2.0, 0.0, 0.0);
-odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
graph.addRelativePose(i1, i2, odometry, odometryNoise);
graph.addRelativePose(i2, i3, odometry, odometryNoise);
%% Add bearing/range measurement factors
degrees = pi/180;
-noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
+noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]);
graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);
diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m
index 861e56db9..560ab7ace 100644
--- a/matlab/examples/PlanarSLAMExample_sampling.m
+++ b/matlab/examples/PlanarSLAMExample_sampling.m
@@ -15,22 +15,22 @@
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
graph = planarSLAMGraph;
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
+priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
odometry = gtsamPose2(2.0, 0.0, 0.0);
-odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
graph.addRelativePose(i1, i2, odometry, odometryNoise);
graph.addRelativePose(i2, i3, odometry, odometryNoise);
%% Except, for measurements we offer a choice
j1 = symbol('l',1); j2 = symbol('l',2);
degrees = pi/180;
-noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
+noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]);
if 1
graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
else
- bearingModel = gtsamnoiseModelDiagonal_Sigmas(0.1);
+ bearingModel = gtsamnoiseModelDiagonal.Sigmas(0.1);
graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel);
graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel);
end
diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/examples/Pose2SLAMExample.m
index 776260e3f..9cb5b8af4 100644
--- a/matlab/examples/Pose2SLAMExample.m
+++ b/matlab/examples/Pose2SLAMExample.m
@@ -24,19 +24,19 @@ graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
+priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry
% general noisemodel for odometry
-odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint
-model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
+model = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
% print
diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/examples/Pose2SLAMExample_advanced.m
index 843ae7e1c..45d288e50 100644
--- a/matlab/examples/Pose2SLAMExample_advanced.m
+++ b/matlab/examples/Pose2SLAMExample_advanced.m
@@ -25,20 +25,20 @@ graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
-priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
+priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry
% general noisemodel for odometry
-odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
%% Add measurements
% general noisemodel for measurements
-measurementNoise = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
+measurementNoise = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]);
% print
graph.print('full graph');
diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m
index e05a77186..c089f5949 100644
--- a/matlab/examples/Pose2SLAMExample_circle.m
+++ b/matlab/examples/Pose2SLAMExample_circle.m
@@ -11,7 +11,7 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
-hexagon = pose2SLAMValues_Circle(6,1.0);
+hexagon = pose2SLAMValues.Circle(6,1.0);
p0 = hexagon.pose(0);
p1 = hexagon.pose(1);
@@ -19,7 +19,7 @@ p1 = hexagon.pose(1);
fg = pose2SLAMGraph;
fg.addPoseConstraint(0, p0);
delta = p0.between(p1);
-covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 5*pi/180]);
+covariance = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 5*pi/180]);
fg.addRelativePose(0,1, delta, covariance);
fg.addRelativePose(1,2, delta, covariance);
fg.addRelativePose(2,3, delta, covariance);
diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/examples/Pose2SLAMExample_graph.m
index a2645a593..258730aaa 100644
--- a/matlab/examples/Pose2SLAMExample_graph.m
+++ b/matlab/examples/Pose2SLAMExample_graph.m
@@ -11,13 +11,13 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Initialize graph, initial estimate, and odometry noise
-model = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 5*pi/180]);
+model = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 5*pi/180]);
[graph,initial]=load2D('../../examples/Data/w100-odom.graph',model);
initial.print(sprintf('Initial estimate:\n'));
%% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
-priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.01; 0.01; 0.01]);
+priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.01; 0.01; 0.01]);
graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph
%% Plot Initial Estimate
diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/examples/Pose2SLAMwSPCG.m
index 3d0ce8da7..3e773689b 100644
--- a/matlab/examples/Pose2SLAMwSPCG.m
+++ b/matlab/examples/Pose2SLAMwSPCG.m
@@ -22,19 +22,19 @@ graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
+priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry
% general noisemodel for odometry
-odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint
-model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
+model = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
% print
diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m
index c335203eb..c1328585e 100644
--- a/matlab/examples/Pose3SLAMExample.m
+++ b/matlab/examples/Pose3SLAMExample.m
@@ -11,7 +11,7 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
-hexagon = pose3SLAMValues_Circle(6,1.0);
+hexagon = pose3SLAMValues.Circle(6,1.0);
p0 = hexagon.pose(0);
p1 = hexagon.pose(1);
@@ -19,7 +19,7 @@ p1 = hexagon.pose(1);
fg = pose3SLAMGraph;
fg.addPoseConstraint(0, p0);
delta = p0.between(p1);
-covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+covariance = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
fg.addRelativePose(0,1, delta, covariance);
fg.addRelativePose(1,2, delta, covariance);
fg.addRelativePose(2,3, delta, covariance);
diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m
index f2c411d08..f39fc306b 100644
--- a/matlab/examples/Pose3SLAMExample_graph.m
+++ b/matlab/examples/Pose3SLAMExample_graph.m
@@ -16,7 +16,7 @@ N = 2500;
filename = '../../examples/Data/sphere2500.txt';
%% Initialize graph, initial estimate, and odometry noise
-model = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+model = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
[graph,initial]=load3D(filename,model,true,N);
%% Plot Initial Estimate
diff --git a/matlab/examples/SBAExample.m b/matlab/examples/SBAExample.m
index 9050d011d..82fdd4646 100644
--- a/matlab/examples/SBAExample.m
+++ b/matlab/examples/SBAExample.m
@@ -34,7 +34,7 @@ graph = sparseBAGraph;
%% Add factors for all measurements
-measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma);
+measurementNoise = gtsamnoiseModelIsotropic.Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z)
for k=1:length(data.Z{i})
j = data.J{i}{k};
@@ -43,11 +43,11 @@ for i=1:length(data.Z)
end
%% Add Gaussian priors for a pose and a landmark to constrain the system
-cameraPriorNoise = gtsamnoiseModelDiagonal_Sigmas(cameraNoiseSigmas);
+cameraPriorNoise = gtsamnoiseModelDiagonal.Sigmas(cameraNoiseSigmas);
firstCamera = gtsamSimpleCamera(truth.cameras{1}.pose, truth.K);
graph.addSimpleCameraPrior(symbol('c',1), firstCamera, cameraPriorNoise);
-pointPriorNoise = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma);
+pointPriorNoise = gtsamnoiseModelIsotropic.Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
%% Print the graph
diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m
index 277f31648..3eef9f916 100644
--- a/matlab/examples/SFMExample.m
+++ b/matlab/examples/SFMExample.m
@@ -32,7 +32,7 @@ poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
graph = visualSLAMGraph;
%% Add factors for all measurements
-measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma);
+measurementNoise = gtsamnoiseModelIsotropic.Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z)
for k=1:length(data.Z{i})
j = data.J{i}{k};
@@ -41,9 +41,9 @@ for i=1:length(data.Z)
end
%% Add Gaussian priors for a pose and a landmark to constrain the system
-posePriorNoise = gtsamnoiseModelDiagonal_Sigmas(poseNoiseSigmas);
+posePriorNoise = gtsamnoiseModelDiagonal.Sigmas(poseNoiseSigmas);
graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise);
-pointPriorNoise = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma);
+pointPriorNoise = gtsamnoiseModelIsotropic.Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
%% Print the graph
diff --git a/matlab/examples/StereoVOExample.m b/matlab/examples/StereoVOExample.m
index 99adaa82b..9cf838e94 100644
--- a/matlab/examples/StereoVOExample.m
+++ b/matlab/examples/StereoVOExample.m
@@ -31,7 +31,7 @@ graph.addPoseConstraint(x1, first_pose);
%% Create realistic calibration and measurement noise model
% format: fx fy skew cx cy baseline
K = gtsamCal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
-stereo_model = gtsamnoiseModelDiagonal_Sigmas([1.0; 1.0; 1.0]);
+stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]);
%% Add measurements
% pose 1
diff --git a/matlab/examples/StereoVOExample_large.m b/matlab/examples/StereoVOExample_large.m
index ef1211e2a..d57628fa1 100644
--- a/matlab/examples/StereoVOExample_large.m
+++ b/matlab/examples/StereoVOExample_large.m
@@ -14,7 +14,7 @@
% format: fx fy skew cx cy baseline
calib = dlmread('../../examples/Data/VO_calibration.txt');
K = gtsamCal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
-stereo_model = gtsamnoiseModelDiagonal_Sigmas([1.0; 1.0; 1.0]);
+stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]);
%% create empty graph and values
graph = visualSLAMGraph;
diff --git a/matlab/load3D.m b/matlab/load3D.m
index 6ba861a92..b457ea1af 100644
--- a/matlab/load3D.m
+++ b/matlab/load3D.m
@@ -29,7 +29,7 @@ for i=1:n
i=v{2};
if (~successive & i
@@ -64,7 +65,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << cppType << " " << name << " = unwrap< ";
file.oss << cppType << " >(" << matlabName;
- if (is_ptr || is_ref) file.oss << ", \"" << matlabType << "\"";
+ if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabType << "\"";
file.oss << ");" << endl;
}
diff --git a/wrap/Argument.h b/wrap/Argument.h
index b4c3fe4a7..4dcf6b563 100644
--- a/wrap/Argument.h
+++ b/wrap/Argument.h
@@ -14,6 +14,7 @@
* @brief arguments to constructors and methods
* @author Frank Dellaert
* @author Andrew Melim
+ * @author Richard Roberts
**/
#pragma once
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index 866ee7a3f..14d9a1f84 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -1,6 +1,6 @@
# Build/install Wrap
-find_package(Boost 1.42 COMPONENTS system filesystem thread REQUIRED)
+set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY})
# Build the executable itself
file(GLOB wrap_srcs "*.cpp")
@@ -9,19 +9,21 @@ list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp)
add_library(wrap_lib STATIC ${wrap_srcs} ${wrap_headers})
gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers})
add_executable(wrap wrap.cpp)
-target_link_libraries(wrap wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
+target_link_libraries(wrap wrap_lib ${WRAP_BOOST_LIBRARIES})
-# Install wrap binary
+# Install wrap binary and export target
if (GTSAM_INSTALL_WRAP)
- install(TARGETS wrap DESTINATION bin)
+ install(TARGETS wrap EXPORT GTSAM-exports DESTINATION bin)
+ list(APPEND GTSAM_EXPORTED_TARGETS wrap)
+ set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
endif(GTSAM_INSTALL_WRAP)
# Install matlab header
install(FILES matlab.h DESTINATION include/wrap)
# Build tests
-if (GTSAM_BUILD_TESTS)
- set(wrap_local_libs wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
+if (GTSAM_BUILD_TESTS)
+ set(wrap_local_libs wrap_lib ${WRAP_BOOST_LIBRARIES})
gtsam_add_subdir_tests("wrap" "${wrap_local_libs}" "${wrap_local_libs}" "")
endif(GTSAM_BUILD_TESTS)
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index aae97de37..2cb36386a 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -1,176 +1,317 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.cpp
- * @author Frank Dellaert
- * @author Andrew Melim
- **/
-
-#include
-#include
-#include
-
-#include
-
-#include "Class.h"
-#include "utilities.h"
-#include "Argument.h"
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-void Class::matlab_proxy(const string& classFile) const {
- // open destination classFile
- FileWriter file(classFile, verbose_, "%");
-
- // get the name of actual matlab object
- string matlabName = qualifiedName();
-
- // emit class proxy code
- // we want our class to inherit the handle class for memory purposes
- file.oss << "classdef " << matlabName << " < handle" << endl;
- file.oss << " properties" << endl;
- file.oss << " self = 0" << endl;
- file.oss << " end" << endl;
- file.oss << " methods" << endl;
- // constructor
- file.oss << " function obj = " << matlabName << "(varargin)" << endl;
- //i is constructor id
- int id = 0;
- BOOST_FOREACH(ArgumentList a, constructor.args_list)
- {
- constructor.matlab_proxy_fragment(file,matlabName, id, a);
- id++;
- }
- //Static constructor collect call
- file.oss << " if nargin ==14, new_" << matlabName << "(varargin{1},0); end" << endl;
- file.oss << " if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl;
- file.oss << " end" << endl;
- // deconstructor
- file.oss << " function delete(obj)" << endl;
- file.oss << " if obj.self ~= 0" << endl;
- //TODO: Add verbosity flag
- //file.oss << " fprintf(1,'MATLAB class deleting %x',obj.self);" << endl;
- file.oss << " new_" << matlabName << "(obj.self);" << endl;
- file.oss << " obj.self = 0;" << endl;
- file.oss << " end" << endl;
- file.oss << " end" << endl;
- file.oss << " function display(obj), obj.print(''); end" << endl;
- file.oss << " function disp(obj), obj.display; end" << endl;
- file.oss << " end" << endl;
- file.oss << "end" << endl;
-
- // close file
- file.emit(true);
-}
-
-/* ************************************************************************* */
-//TODO: Consolidate into single file
-void Class::matlab_constructors(const string& toolboxPath) const {
- /*BOOST_FOREACH(Constructor c, constructors) {
- args_list.push_back(c.args);
- }*/
-
- BOOST_FOREACH(ArgumentList a, constructor.args_list) {
- constructor.matlab_mfile(toolboxPath, qualifiedName(), a);
- }
- constructor.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(),
- using_namespaces, includes);
-}
-
-/* ************************************************************************* */
-void Class::matlab_methods(const string& classPath) const {
- string matlabName = qualifiedName(), cppName = qualifiedName("::");
- BOOST_FOREACH(Method m, methods) {
- m.matlab_mfile (classPath);
- m.matlab_wrapper(classPath, name, cppName, matlabName, using_namespaces, includes);
- }
-}
-
-/* ************************************************************************* */
-void Class::matlab_static_methods(const string& toolboxPath) 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(FileWriter& file,
- const string& toolboxPath,
- const string& mexFlags) const {
- string mex = "mex " + mexFlags + " ";
- string matlabClassName = qualifiedName();
- file.oss << mex << constructor.matlab_wrapper_name(matlabClassName) << ".cpp" << endl;
- BOOST_FOREACH(StaticMethod sm, static_methods)
- file.oss << mex << matlabClassName + "_" + sm.name << ".cpp" << endl;
- file.oss << endl << "cd @" << matlabClassName << endl;
- BOOST_FOREACH(Method m, methods)
- file.oss << mex << m.name << ".cpp" << endl;
- file.oss << endl;
-}
-
-/* ************************************************************************* */
-void Class::makefile_fragment(FileWriter& file) 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 file_names;
- string file_base = constructor.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) {
- file.oss << file_base << ".$(MEXENDING): " << file_base << ".cpp";
- file.oss << " $(PATH_TO_WRAP)/matlab.h" << endl;
- file.oss << "\t$(MEX) $(mex_flags) " << file_base << ".cpp -output " << file_base << endl;
- }
-
- // class target
- file.oss << "\n" << matlabName << ": ";
- BOOST_FOREACH(const string& file_base, file_names) {
- file.oss << file_base << ".$(MEXENDING) ";
- }
- file.oss << "\n" << endl;
-}
-
-/* ************************************************************************* */
-string Class::qualifiedName(const string& delim) const {
- string result;
- BOOST_FOREACH(const string& ns, namespaces)
- result += ns + delim;
- return result + name;
-}
-
-/* ************************************************************************* */
+/* ----------------------------------------------------------------------------
+
+ * 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 Class.cpp
+ * @author Frank Dellaert
+ * @author Andrew Melim
+ * @author Richard Roberts
+ **/
+
+#include
+#include
+#include
+//#include // on Linux GCC: fails with error regarding needing C++0x std flags
+//#include // same failure as above
+#include // works on Linux GCC
+
+#include
+#include
+
+#include "Class.h"
+#include "utilities.h"
+#include "Argument.h"
+
+using namespace std;
+using namespace wrap;
+
+/* ************************************************************************* */
+void Class::matlab_proxy(const string& classFile, const string& wrapperName,
+ const TypeAttributesTable& typeAttributes,
+ FileWriter& wrapperFile, vector& functionNames) const {
+ // open destination classFile
+ FileWriter proxyFile(classFile, verbose_, "%");
+
+ // get the name of actual matlab object
+ const string matlabName = qualifiedName(), cppName = qualifiedName("::");
+ const string matlabBaseName = wrap::qualifiedName("", qualifiedParent);
+ const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
+
+ // emit class proxy code
+ // we want our class to inherit the handle class for memory purposes
+ const string parent = qualifiedParent.empty() ?
+ "handle" : ::wrap::qualifiedName("", qualifiedParent);
+ proxyFile.oss << "classdef " << matlabName << " < " << parent << endl;
+ proxyFile.oss << " properties" << endl;
+ proxyFile.oss << " ptr_" << matlabName << " = 0" << endl;
+ proxyFile.oss << " end" << endl;
+ proxyFile.oss << " methods" << endl;
+
+ // Constructor
+ proxyFile.oss << " function obj = " << matlabName << "(varargin)" << endl;
+ // Special pointer constructors - one in MATLAB to create an object and
+ // assign a pointer returned from a C++ function. In turn this MATLAB
+ // constructor calls a special C++ function that just adds the object to
+ // its collector. This allows wrapped functions to return objects in
+ // other wrap modules - to add these to their collectors the pointer is
+ // passed from one C++ module into matlab then back into the other C++
+ // module.
+ pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
+ wrapperFile.oss << "\n";
+ // Regular constructors
+ BOOST_FOREACH(ArgumentList a, constructor.args_list)
+ {
+ const int id = (int)functionNames.size();
+ constructor.proxy_fragment(proxyFile, wrapperName, matlabName, matlabBaseName, id, a);
+ const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
+ cppName, matlabName, cppBaseName, id, using_namespaces, a);
+ wrapperFile.oss << "\n";
+ functionNames.push_back(wrapFunctionName);
+ }
+ proxyFile.oss << " else\n";
+ proxyFile.oss << " error('Arguments do not match any overload of " << matlabName << " constructor');" << endl;
+ proxyFile.oss << " end\n";
+ if(!qualifiedParent.empty())
+ proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n";
+ proxyFile.oss << " obj.ptr_" << matlabName << " = my_ptr;\n";
+ proxyFile.oss << " end\n\n";
+
+ // Deconstructor
+ {
+ const int id = (int)functionNames.size();
+ deconstructor.proxy_fragment(proxyFile, wrapperName, matlabName, id);
+ proxyFile.oss << "\n";
+ const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabName, id, using_namespaces);
+ wrapperFile.oss << "\n";
+ functionNames.push_back(functionName);
+ }
+ proxyFile.oss << " function display(obj), obj.print(''); end\n\n";
+ proxyFile.oss << " function disp(obj), obj.display; end\n\n";
+
+ // Methods
+ BOOST_FOREACH(const Methods::value_type& name_m, methods) {
+ const Method& m = name_m.second;
+ m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabName, wrapperName, using_namespaces, typeAttributes, functionNames);
+ proxyFile.oss << "\n";
+ wrapperFile.oss << "\n";
+ }
+
+ proxyFile.oss << " end\n";
+ proxyFile.oss << "\n";
+ proxyFile.oss << " methods(Static = true)\n";
+
+ // Static methods
+ BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
+ const StaticMethod& m = name_m.second;
+ m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabName, wrapperName, using_namespaces, typeAttributes, functionNames);
+ proxyFile.oss << "\n";
+ wrapperFile.oss << "\n";
+ }
+
+ proxyFile.oss << " end" << endl;
+ proxyFile.oss << "end" << endl;
+
+ // Close file
+ proxyFile.emit(true);
+}
+
+/* ************************************************************************* */
+string Class::qualifiedName(const string& delim) const {
+ return ::wrap::qualifiedName(delim, namespaces, name);
+}
+
+/* ************************************************************************* */
+void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector& functionNames) const {
+
+ const string matlabName = qualifiedName(), cppName = qualifiedName("::");
+ const string baseMatlabName = wrap::qualifiedName("", qualifiedParent);
+ const string baseCppName = wrap::qualifiedName("::", qualifiedParent);
+
+ const int collectorInsertId = (int)functionNames.size();
+ const string collectorInsertFunctionName = matlabName + "_collectorInsertAndMakeBase_" + boost::lexical_cast(collectorInsertId);
+ functionNames.push_back(collectorInsertFunctionName);
+
+ int upcastFromVoidId;
+ string upcastFromVoidFunctionName;
+ if(isVirtual) {
+ upcastFromVoidId = (int)functionNames.size();
+ upcastFromVoidFunctionName = matlabName + "_upcastFromVoid_" + boost::lexical_cast(upcastFromVoidId);
+ functionNames.push_back(upcastFromVoidFunctionName);
+ }
+
+ // MATLAB constructor that assigns pointer to matlab object then calls c++
+ // function to add the object to the collector.
+ if(isVirtual) {
+ proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))";
+ } else {
+ proxyFile.oss << " if nargin == 2";
+ }
+ proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" << ptr_constructor_key << ")\n";
+ if(isVirtual) {
+ proxyFile.oss << " if nargin == 2\n";
+ proxyFile.oss << " my_ptr = varargin{2};\n";
+ proxyFile.oss << " else\n";
+ proxyFile.oss << " my_ptr = " << wrapperName << "(" << upcastFromVoidId << ", varargin{2});\n";
+ proxyFile.oss << " end\n";
+ } else {
+ proxyFile.oss << " my_ptr = varargin{2};\n";
+ }
+ if(qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back
+ proxyFile.oss << " ";
+ else
+ proxyFile.oss << " base_ptr = ";
+ proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr
+
+ // C++ function to add pointer from MATLAB to collector. The pointer always
+ // comes from a C++ return value; this mechanism allows the object to be added
+ // to a collector in a different wrap module. If this class has a base class,
+ // a new pointer to the base class is allocated and returned.
+ wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
+ wrapperFile.oss << "{\n";
+ wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n";
+ generateUsingNamespace(wrapperFile, using_namespaces);
+ // Typedef boost::shared_ptr
+ wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n";
+ wrapperFile.oss << "\n";
+ // Get self pointer passed in
+ wrapperFile.oss << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n";
+ // Add to collector
+ wrapperFile.oss << " collector_" << matlabName << ".insert(self);\n";
+ // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
+ if(!qualifiedParent.empty()) {
+ wrapperFile.oss << "\n";
+ wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName << "> SharedBase;\n";
+ wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
+ wrapperFile.oss << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n";
+ }
+ wrapperFile.oss << "}\n";
+
+ // If this is a virtual function, C++ function to dynamic upcast it from a
+ // shared_ptr. This mechanism allows automatic dynamic creation of the
+ // real underlying derived-most class when a C++ method returns a virtual
+ // base class.
+ if(isVirtual)
+ wrapperFile.oss <<
+ "\n"
+ "void " << upcastFromVoidFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n"
+ " mexAtExit(&_deleteAllObjects);\n"
+ " typedef boost::shared_ptr<" << cppName << "> Shared;\n"
+ " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n"
+ " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"
+ " Shared *self = new Shared(boost::static_pointer_cast<" << cppName << ">(*asVoid));\n"
+ " *reinterpret_cast(mxGetData(out[0])) = self;\n"
+ "}\n";
+}
+
+/* ************************************************************************* */
+vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName) {
+ vector result;
+ BOOST_FOREACH(const ArgumentList& argList, argLists) {
+ ArgumentList instArgList;
+ BOOST_FOREACH(const Argument& arg, argList) {
+ Argument instArg = arg;
+ if(arg.type == templateArg) {
+ instArg.namespaces.assign(instName.begin(), instName.end()-1);
+ instArg.type = instName.back();
+ }
+ instArgList.push_back(instArg);
+ }
+ result.push_back(instArgList);
+ }
+ return result;
+}
+
+/* ************************************************************************* */
+template
+map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName) {
+ map result;
+ typedef pair Name_Method;
+ BOOST_FOREACH(const Name_Method& name_method, methods) {
+ const METHOD& method = name_method.second;
+ METHOD instMethod = method;
+ instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName);
+ instMethod.returnVals.clear();
+ BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) {
+ ReturnValue instRetVal = retVal;
+ if(retVal.type1 == templateArg) {
+ instRetVal.namespaces1.assign(instName.begin(), instName.end()-1);
+ instRetVal.type1 = instName.back();
+ }
+ if(retVal.type2 == templateArg) {
+ instRetVal.namespaces2.assign(instName.begin(), instName.end()-1);
+ instRetVal.type2 = instName.back();
+ }
+ instMethod.returnVals.push_back(instRetVal);
+ }
+ result.insert(make_pair(name_method.first, instMethod));
+ }
+ return result;
+}
+
+/* ************************************************************************* */
+Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName) {
+ Class inst;
+ inst.name = cls.name;
+ inst.templateArgs = cls.templateArgs;
+ inst.typedefName = cls.typedefName;
+ inst.isVirtual = cls.isVirtual;
+ inst.qualifiedParent = cls.qualifiedParent;
+ inst.methods = expandMethodTemplate(cls.methods, templateArg, instName);
+ inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName);
+ inst.namespaces = cls.namespaces;
+ inst.using_namespaces = cls.using_namespaces;
+ bool allIncludesEmpty = true;
+ BOOST_FOREACH(const string& inc, cls.includes) { if(!inc.empty()) { allIncludesEmpty = false; break; } }
+ if(allIncludesEmpty)
+ inst.includes.push_back(cls.name + ".h");
+ else
+ inst.includes = cls.includes;
+ inst.constructor = cls.constructor;
+ inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName);
+ inst.constructor.name = inst.name;
+ inst.deconstructor = cls.deconstructor;
+ inst.deconstructor.name = inst.name;
+ inst.verbose_ = cls.verbose_;
+ return inst;
+}
+
+/* ************************************************************************* */
+vector Class::expandTemplate(const string& templateArg, const vector >& instantiations) const {
+ vector result;
+ BOOST_FOREACH(const vector& instName, instantiations) {
+ Class inst = expandClassTemplate(*this, templateArg, instName);
+ inst.name = name + instName.back();
+ inst.templateArgs.clear();
+ inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">";
+ result.push_back(inst);
+ }
+ return result;
+}
+
+/* ************************************************************************* */
+Class Class::expandTemplate(const string& templateArg, const vector& instantiation) const {
+ return expandClassTemplate(*this, templateArg, instantiation);
+}
+
+/* ************************************************************************* */
+std::string Class::getTypedef() const {
+ string result;
+ BOOST_FOREACH(const string& namesp, namespaces) {
+ result += ("namespace " + namesp + " { ");
+ }
+ result += ("typedef " + typedefName + " " + name + ";");
+ BOOST_FOREACH(const string& namesp, namespaces) {
+ result += " }";
+ }
+ return result;
+}
+
+/* ************************************************************************* */
diff --git a/wrap/Class.h b/wrap/Class.h
index fb9906716..beee2c29f 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -14,44 +14,58 @@
* @brief describe the C++ class that is being wrapped
* @author Frank Dellaert
* @author Andrew Melim
+ * @author Richard Roberts
**/
#pragma once
#include
+#include