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

Merge branch 'develop' into feature/BAD_using_charts

release/4.3a0
Paul Furgale 2014-11-22 14:38:55 +01:00
parent e5fe5676b1 018e66be7f
commit aae206b308
112 changed files with 4986 additions and 1901 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
.cproject
View File

@ -2353,6 +2353,38 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testMethod.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testMethod.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testClass.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testClass.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -3222,22 +3254,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

4
CMakeLists.txt
View File

@ -9,8 +9,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MINOR 1)
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")

11
examples/Data/pose3example-rewritten.txt
View File

@ -1,11 +0,0 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488
EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.30338 -0.513226 0.542221 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000

126
gtsam.h
View File

@ -156,8 +156,14 @@ virtual class Value {
size_t dim() const;
};
class Vector3 {
Vector3(Vector v);
};
class Vector6 {
Vector6(Vector v);
};
#include <gtsam/base/LieScalar.h>
virtual class LieScalar : gtsam::Value {
class LieScalar {
// Standard constructors
LieScalar();
LieScalar(double d);
@ -186,7 +192,7 @@ virtual class LieScalar : gtsam::Value {
};
#include <gtsam/base/LieVector.h>
virtual class LieVector : gtsam::Value {
class LieVector {
// Standard constructors
LieVector();
LieVector(Vector v);
@ -218,7 +224,7 @@ virtual class LieVector : gtsam::Value {
};
#include <gtsam/base/LieMatrix.h>
virtual class LieMatrix : gtsam::Value {
class LieMatrix {
// Standard constructors
LieMatrix();
LieMatrix(Matrix v);
@ -253,7 +259,7 @@ virtual class LieMatrix : gtsam::Value {
// geometry
//*************************************************************************
virtual class Point2 : gtsam::Value {
class Point2 {
// Standard Constructors
Point2();
Point2(double x, double y);
@ -290,7 +296,7 @@ virtual class Point2 : gtsam::Value {
void serialize() const;
};
virtual class StereoPoint2 : gtsam::Value {
class StereoPoint2 {
// Standard Constructors
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
@ -325,7 +331,7 @@ virtual class StereoPoint2 : gtsam::Value {
void serialize() const;
};
virtual class Point3 : gtsam::Value {
class Point3 {
// Standard Constructors
Point3();
Point3(double x, double y, double z);
@ -361,7 +367,7 @@ virtual class Point3 : gtsam::Value {
void serialize() const;
};
virtual class Rot2 : gtsam::Value {
class Rot2 {
// Standard Constructors and Named Constructors
Rot2();
Rot2(double theta);
@ -406,7 +412,7 @@ virtual class Rot2 : gtsam::Value {
void serialize() const;
};
virtual class Rot3 : gtsam::Value {
class Rot3 {
// Standard Constructors and Named Constructors
Rot3();
Rot3(Matrix R);
@ -462,7 +468,7 @@ virtual class Rot3 : gtsam::Value {
void serialize() const;
};
virtual class Pose2 : gtsam::Value {
class Pose2 {
// Standard Constructor
Pose2();
Pose2(const gtsam::Pose2& pose);
@ -512,7 +518,7 @@ virtual class Pose2 : gtsam::Value {
void serialize() const;
};
virtual class Pose3 : gtsam::Value {
class Pose3 {
// Standard Constructors
Pose3();
Pose3(const gtsam::Pose3& pose);
@ -564,7 +570,7 @@ virtual class Pose3 : gtsam::Value {
};
#include <gtsam/geometry/Unit3.h>
virtual class Unit3 : gtsam::Value {
class Unit3 {
// Standard Constructors
Unit3();
Unit3(const gtsam::Point3& pose);
@ -585,7 +591,7 @@ virtual class Unit3 : gtsam::Value {
};
#include <gtsam/geometry/EssentialMatrix.h>
virtual class EssentialMatrix : gtsam::Value {
class EssentialMatrix {
// Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
@ -606,7 +612,7 @@ virtual class EssentialMatrix : gtsam::Value {
double error(Vector vA, Vector vB);
};
virtual class Cal3_S2 : gtsam::Value {
class Cal3_S2 {
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
@ -643,7 +649,7 @@ virtual class Cal3_S2 : gtsam::Value {
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Value {
class Cal3DS2 {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
@ -699,7 +705,43 @@ class Cal3_S2Stereo {
double baseline() const;
};
virtual class CalibratedCamera : gtsam::Value {
#include <gtsam/geometry/Cal3Bundler.h>
class Cal3Bundler {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3Bundler retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// Standard Interface
double fx() const;
double fy() const;
double k1() const;
double k2() const;
double u0() const;
double v0() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
// enabling serialization functionality
void serialize() const;
};
class CalibratedCamera {
// Standard Constructors and Named Constructors
CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose);
@ -732,7 +774,7 @@ virtual class CalibratedCamera : gtsam::Value {
void serialize() const;
};
virtual class SimpleCamera : gtsam::Value {
class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
@ -771,7 +813,7 @@ virtual class SimpleCamera : gtsam::Value {
};
template<CALIBRATION = {gtsam::Cal3DS2}>
virtual class PinholeCamera : gtsam::Value {
class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::Pose3& pose);
@ -809,7 +851,7 @@ virtual class PinholeCamera : gtsam::Value {
void serialize() const;
};
virtual class StereoCamera : gtsam::Value {
class StereoCamera {
// Standard Constructors and Named Constructors
StereoCamera();
StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
@ -862,7 +904,7 @@ virtual class SymbolicFactor {
};
#include <gtsam/symbolic/SymbolicFactorGraph.h>
class SymbolicFactorGraph {
virtual class SymbolicFactorGraph {
SymbolicFactorGraph();
SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
@ -1664,15 +1706,12 @@ class Values {
void print(string s) const;
bool equals(const gtsam::Values& other, double tol) const;
void insert(size_t j, const gtsam::Value& value);
void insert(const gtsam::Values& values);
void update(size_t j, const gtsam::Value& val);
void update(const gtsam::Values& values);
void erase(size_t j);
void swap(gtsam::Values& values);
bool exists(size_t j) const;
gtsam::Value at(size_t j) const;
gtsam::KeyList keys() const;
gtsam::VectorValues zeroVectors() const;
@ -1682,6 +1721,37 @@ class Values {
// enabling serialization functionality
void serialize() const;
// New in 4.0, we have to specialize every insert/update/at to generate wrappers
// Instead of the old:
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
void insert(size_t j, const gtsam::Pose2& t);
void insert(size_t j, const gtsam::Rot3& t);
void insert(size_t j, const gtsam::Pose3& t);
void insert(size_t j, const gtsam::Cal3_S2& t);
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
void update(size_t j, const gtsam::Pose2& t);
void update(size_t j, const gtsam::Rot3& t);
void update(size_t j, const gtsam::Pose3& t);
void update(size_t j, const gtsam::Cal3_S2& t);
void update(size_t j, const gtsam::Cal3DS2& t);
void update(size_t j, const gtsam::Cal3Bundler& t);
void update(size_t j, const gtsam::EssentialMatrix& t);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2}>
T at(size_t j);
};
// Actually a FastList<Key>
@ -2077,7 +2147,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2088,7 +2158,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const;
@ -2099,7 +2169,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible);
@ -2280,7 +2350,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph,
namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
virtual class ConstantBias : gtsam::Value {
class ConstantBias {
// Standard Constructor
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
@ -2340,7 +2410,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
@ -2383,7 +2453,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;

4
gtsam/3rdparty/gtsam_eigen_includes.h.in vendored
View File

@ -27,3 +27,7 @@
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry>

12
gtsam/base/GenericValue.h
View File

@ -70,9 +70,9 @@ struct print<double> {
};
// equals for Matrix types
template<int M, int N, int Options>
struct equals<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, M, N, Options> type;
template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct equals<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> type;
typedef bool result_type;
bool operator()(const type& A, const type& B, double tol) {
return equal_with_abs_tol(A, B, tol);
@ -80,9 +80,9 @@ struct equals<Eigen::Matrix<double, M, N, Options> > {
};
// print for Matrix types
template<int M, int N, int Options>
struct print<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, M, N, Options> type;
template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct print<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> type;
typedef void result_type;
void operator()(const type& A, const std::string& str) {
std::cout << str << ": " << A << std::endl;

4
gtsam/base/Manifold.h
View File

@ -253,10 +253,10 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
"DefaultChart has not been implemented yet for dynamically sized matrices");
static vector local(const T& origin, const T& other) {
return reshape<vector::RowsAtCompileTime, 1>(other) - reshape<vector::RowsAtCompileTime, 1>(origin);
return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other) - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
}
static T retract(const T& origin, const vector& d) {
return origin + reshape<M, N>(d);
return origin + reshape<M, N, Options>(d);
}
static int getDimension(const T&origin) {
return origin.rows() * origin.cols();

16
gtsam/base/Matrix.h
View File

@ -294,10 +294,10 @@ void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
inline Matrix trans(const Matrix& A) { return A.transpose(); }
/// Reshape functor
template <int OutM, int OutN, int InM, int InN, int InOptions>
template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
struct Reshape {
//TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff)
typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN> > ReshapedType;
typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN, OutOptions> > ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
return in.data();
}
@ -305,7 +305,7 @@ struct Reshape {
/// Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output)
template <int M, int InOptions>
struct Reshape<M, M, M, M, InOptions> {
struct Reshape<M, M, InOptions, M, M, InOptions> {
typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
return in;
@ -314,7 +314,7 @@ struct Reshape<M, M, M, M, InOptions> {
/// Reshape specialization that does nothing as shape stays the same
template <int M, int N, int InOptions>
struct Reshape<M, N, M, N, InOptions> {
struct Reshape<M, N, InOptions, M, N, InOptions> {
typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
return in;
@ -323,17 +323,17 @@ struct Reshape<M, N, M, N, InOptions> {
/// Reshape specialization that does transpose
template <int M, int N, int InOptions>
struct Reshape<N, M, M, N, InOptions> {
struct Reshape<N, M, InOptions, M, N, InOptions> {
typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
return in.transpose();
}
};
template <int OutM, int OutN, int InM, int InN, int InOptions>
inline typename Reshape<OutM, OutN, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
return Reshape<OutM, OutN, InM, InN, InOptions>::reshape(m);
return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
}
/**

11
gtsam/geometry/Cal3DS2.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> {
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
typedef Cal3DS2_Base Base;
@ -87,21 +87,24 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable
/// @}
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Cal3DS2_Base>(*this));
}
/// @}
};
// Define GTSAM traits

47
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
static Matrix29 D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) {
Eigen::Matrix<double, 2, 5> DR1;
const Matrix2& DK) {
Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2;
Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D;
Matrix29 D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
static Matrix2 D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) {
const Matrix2& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@ -82,7 +82,7 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR;
Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
@ -90,8 +90,9 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix29&> H1,
boost::optional<Matrix2&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
@ -110,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
@ -125,6 +126,26 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1 || H2) {
Matrix29 D1;
Matrix2 D2;
Point2 pu = uncalibrate(p, D1, D2);
if (H1)
*H1 = D1;
if (H2)
*H2 = D2;
return pu;
} else {
return uncalibrate(p);
}
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
@ -161,7 +182,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK;
Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
@ -176,7 +197,7 @@ Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}

11
gtsam/geometry/Cal3DS2_Base.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -114,9 +113,14 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
boost::optional<Matrix29&> Dcal = boost::none,
boost::optional<Matrix2&> Dp = boost::none) const ;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
@ -127,7 +131,6 @@ public:
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
private:
/// @}

21
gtsam/geometry/Cal3Unified.cpp
View File

@ -50,6 +50,7 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
}
/* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
@ -70,26 +71,30 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
// Part2: project NPlane point to pixel plane: use Cal3DS2
Point2 m(x,y);
Matrix H1base, H2base; // jacobians from Base class
Matrix29 H1base;
Matrix2 H2base; // jacobians from Base class
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
// Inlined derivative for calibration
if (H1) {
// part1
Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2,
-ys * sqrt_nx * xi_sqrt_nx2);
Matrix DDS2U = H2base * DU;
Vector2 DU;
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
*H1 = collect(2, &H1base, &DDS2U);
H1->resize(2,10);
H1->block<2,9>(0,0) = H1base;
H1->block<2,1>(0,9) = H2base * DU;
}
// Inlined derivative for points
if (H2) {
// part1
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
const double mid = -(xi * xs*ys) * denom;
Matrix DU = (Matrix(2, 2) <<
(sqrt_nx + xi*(ys*ys + 1)) * denom, mid,
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom);
Matrix2 DU;
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
*H2 = H2base * DU;
}

4
gtsam/geometry/Cal3Unified.h
View File

@ -40,7 +40,7 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> {
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
typedef Cal3Unified This;
typedef Cal3DS2_Base Base;
@ -129,8 +129,6 @@ private:
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);

2
gtsam/geometry/EssentialMatrix.h
View File

@ -58,6 +58,8 @@ public:
return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
}
virtual ~EssentialMatrix() {}
/// @}
/// @name Testable

2
gtsam/linear/JacobianFactor.h
View File

@ -187,7 +187,7 @@ namespace gtsam {
/// Return the diagonal of the Hessian for this factor
virtual VectorValues hessianDiagonal() const;
/* ************************************************************************* */
/// Raw memory access version of hessianDiagonal
virtual void hessianDiagonal(double* d) const;
/// Return the block diagonal of the Hessian for this factor

25
gtsam/linear/Preconditioner.cpp
View File

@ -13,6 +13,7 @@
#include <gtsam/linear/NoiseModel.h>
#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/map.hpp>
#include <iostream>
#include <vector>
@ -134,30 +135,16 @@ void BlockJacobiPreconditioner::build(
size_t nnz = 0;
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t dim = dims_[i];
blocks.push_back(Matrix::Zero(dim, dim));
// blocks.push_back(Matrix::Zero(dim, dim));
// nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
nnz += dim*dim;
}
/* compute the block diagonal by scanning over the factors */
/* getting the block diagonals over the factors */
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Ai = jf->getA(it);
blocks[entry.index()] += (Ai.transpose() * Ai);
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Hii = hf->info(it, it).selfadjointView();
blocks[entry.index()] += Hii;
}
}
else {
throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
std::map<Key, Matrix> hessianMap = gf->hessianBlockDiagonal();
BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
blocks.push_back(hessian);
}
/* if necessary, allocating the memory for cacheing the factorization results */

16
gtsam/navigation/ImuFactor.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
class PreintegratedMeasurements {
public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
Matrix measurementCovariance; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
@ -216,11 +216,21 @@ namespace gtsam {
H_vel_pos, H_vel_vel, H_vel_angles,
H_angles_pos, H_angles_vel, H_angles_angles;
// first order uncertainty propagation
// first order uncertainty propagation:
// the deltaT allows to pass from continuous time noise to discrete time noise
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ;
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
//
// Matrix G(9,9);
// G << I_3x3 * deltaT, Z_3x3, Z_3x3,
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
//
// PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){

2
gtsam/nonlinear/Values-inl.h
View File

@ -215,7 +215,7 @@ namespace gtsam {
Values::Values(const Values::ConstFiltered<ValueType>& view) {
BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) {
Key key = key_value.key;
insert<ValueType>(key, key_value.value);
insert(key, static_cast<const ValueType&>(key_value.value));
}
}

6
gtsam/nonlinear/tests/testValues.cpp
View File

@ -383,6 +383,12 @@ TEST(Values, filter) {
expectedSubValues1.insert(3, pose3);
EXPECT(assert_equal(expectedSubValues1, actualSubValues1));
// ConstFilter by Key
Values::ConstFiltered<Value> constfiltered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
EXPECT_LONGS_EQUAL(2, (long)constfiltered.size());
Values fromconstfiltered(constfiltered);
EXPECT(assert_equal(expectedSubValues1, fromconstfiltered));
// Filter by type
i = 0;
Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>();

36
gtsam/slam/ImplicitSchurFactor.h → gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -1,5 +1,5 @@
/**
* @file ImplicitSchurFactor.h
* @file RegularImplicitSchurFactor.h
* @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
* @author Frank Dellaert
* @author Luca Carlone
@ -17,13 +17,13 @@
namespace gtsam {
/**
* ImplicitSchurFactor
* RegularImplicitSchurFactor
*/
template<size_t D> //
class ImplicitSchurFactor: public GaussianFactor {
class RegularImplicitSchurFactor: public GaussianFactor {
public:
typedef ImplicitSchurFactor This; ///< Typedef to this class
typedef RegularImplicitSchurFactor This; ///< Typedef to this class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected:
@ -40,11 +40,11 @@ protected:
public:
/// Constructor
ImplicitSchurFactor() {
RegularImplicitSchurFactor() {
}
/// Construct from blcoks of F, E, inv(E'*E), and RHS vector b
ImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b) :
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
initKeys();
@ -58,7 +58,7 @@ public:
}
/// Destructor
virtual ~ImplicitSchurFactor() {
virtual ~RegularImplicitSchurFactor() {
}
// Write access, only use for construction!
@ -87,7 +87,7 @@ public:
/// print
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << " ImplicitSchurFactor " << std::endl;
std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s);
std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
std::cout << " E_ \n" << E_ << std::endl;
@ -96,7 +96,7 @@ public:
/// equals
bool equals(const GaussianFactor& lf, double tol) const {
if (!dynamic_cast<const ImplicitSchurFactor*>(&lf))
if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
return false;
else {
return false;
@ -110,21 +110,21 @@ public:
virtual Matrix augmentedJacobian() const {
throw std::runtime_error(
"ImplicitSchurFactor::augmentedJacobian non implemented");
"RegularImplicitSchurFactor::augmentedJacobian non implemented");
return Matrix();
}
virtual std::pair<Matrix, Vector> jacobian() const {
throw std::runtime_error("ImplicitSchurFactor::jacobian non implemented");
throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented");
return std::make_pair(Matrix(), Vector());
}
virtual Matrix augmentedInformation() const {
throw std::runtime_error(
"ImplicitSchurFactor::augmentedInformation non implemented");
"RegularImplicitSchurFactor::augmentedInformation non implemented");
return Matrix();
}
virtual Matrix information() const {
throw std::runtime_error(
"ImplicitSchurFactor::information non implemented");
"RegularImplicitSchurFactor::information non implemented");
return Matrix();
}
@ -210,18 +210,18 @@ public:
}
virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_,
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error("ImplicitSchurFactor::clone non implemented");
throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
}
virtual bool empty() const {
return false;
}
virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_,
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error("ImplicitSchurFactor::negate non implemented");
throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented");
}
// Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
@ -454,7 +454,7 @@ public:
}
};
// ImplicitSchurFactor
// RegularImplicitSchurFactor
}

19
gtsam/slam/SmartFactorBase.h
View File

@ -21,7 +21,7 @@
#include "JacobianFactorQ.h"
#include "JacobianFactorSVD.h"
#include "ImplicitSchurFactor.h"
#include "RegularImplicitSchurFactor.h"
#include "RegularHessianFactor.h"
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -73,7 +73,7 @@ public:
/**
* Constructor
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param body_P_sensor is the transform from sensor to body frame (default identity)
*/
SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor) {
@ -271,8 +271,13 @@ public:
Vector bi;
try {
bi =
-(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
if(body_P_sensor_){
Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
Matrix J(6, 6);
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
Fi = Fi * J;
}
} catch (CheiralityException&) {
std::cout << "Cheirality exception " << std::endl;
exit(EXIT_FAILURE);
@ -624,11 +629,11 @@ public:
}
// ****************************************************************************************************
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
typename boost::shared_ptr<ImplicitSchurFactor<D> > f(
new ImplicitSchurFactor<D>());
typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
new RegularImplicitSchurFactor<D>());
computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(),
cameras, point, lambda, diagonalDamping);
f->initKeys();

12
gtsam/slam/SmartProjectionFactor.h
View File

@ -120,7 +120,7 @@ public:
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param body_P_sensor is the transform from sensor to body frame (default identity)
*/
SmartProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
@ -298,7 +298,7 @@ public:
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) {
std::cout << "createImplicitSchurFactor: degenerate configuration"
std::cout << "createRegularImplicitSchurFactor: degenerate configuration"
<< std::endl;
}
return false;
@ -409,12 +409,12 @@ public:
}
// create factor
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createImplicitSchurFactor(cameras, point_, lambda);
return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
else
return boost::shared_ptr<ImplicitSchurFactor<D> >();
return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
}
/// create factor
@ -685,7 +685,7 @@ public:
inline bool isPointBehindCamera() const {
return cheiralityException_;
}
/** return chirality verbosity */
/** return cheirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;
}

5
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -63,7 +63,7 @@ public:
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param body_P_sensor is the transform from sensor to body frame (default identity)
*/
SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
@ -157,6 +157,9 @@ public:
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
if(Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
typename Base::Camera camera(pose, *K_all_[i++]);
cameras.push_back(camera);
}

16
gtsam/slam/tests/testImplicitSchurFactor.cpp → gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -6,7 +6,7 @@
*/
//#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/RegularImplicitSchurFactor.h>
//#include <gtsam_unstable/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQ.h>
//#include "gtsam_unstable/slam/JacobianFactorQR.h"
@ -38,19 +38,19 @@ const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.);
//*************************************************************************************
TEST( implicitSchurFactor, creation ) {
TEST( regularImplicitSchurFactor, creation ) {
// Matrix E = Matrix::Ones(6,3);
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> expected(Fblocks, E, P, b);
RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
Matrix expectedP = expected.getPointCovariance();
EXPECT(assert_equal(expectedP, P));
}
/* ************************************************************************* */
TEST( implicitSchurFactor, addHessianMultiply ) {
TEST( regularImplicitSchurFactor, addHessianMultiply ) {
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
@ -81,11 +81,11 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
keys += 0,1,2,3;
Vector x = xvalues.vector(keys);
Vector expected = zero(24);
ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
// Create ImplicitSchurFactor
ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
{ // First Version
@ -190,7 +190,7 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
}
/* ************************************************************************* */
TEST(implicitSchurFactor, hessianDiagonal)
TEST(regularImplicitSchurFactor, hessianDiagonal)
{
/* TESTED AGAINST MATLAB
* F = [ones(2,6) zeros(2,6) zeros(2,6)
@ -207,7 +207,7 @@ TEST(implicitSchurFactor, hessianDiagonal)
E.block<2,3>(2, 0) << 1,2,3,4,5,6;
E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> factor(Fblocks, E, P, b);
RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
// hessianDiagonal
VectorValues expected;

89
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -292,6 +292,95 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
if(isDebugTest) tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam1(cameraPose1, *K); // with camera poses
SimpleCamera cam2(cameraPose2, *K);
SimpleCamera cam3(cameraPose3, *K);
// create arbitrary body_Pose_sensor (transforms from sensor to body)
Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
// These are the poses we want to estimate, from camera measurements
Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(5, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
double rankTol = 1;
double linThreshold = -1;
bool manageDegeneracy = false;
bool enableEPI = false;
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// Put all factors in factor graph, adding priors
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
// Check errors at ground truth poses
Values gtValues;
gtValues.insert(x1, bodyPose1);
gtValues.insert(x2, bodyPose2);
gtValues.insert(x3, bodyPose3);
double actualError = graph.error(gtValues);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7)
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
Values values;
values.insert(x1, bodyPose1);
values.insert(x2, bodyPose2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, bodyPose3*noise_pose);
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
// result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
// cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;

31
gtsam_unstable/gtsam_unstable.h
View File

@ -4,14 +4,15 @@
// specify the classes from gtsam we are using
virtual class gtsam::Value;
virtual class gtsam::LieScalar;
virtual class gtsam::LieVector;
virtual class gtsam::Point2;
virtual class gtsam::Rot2;
virtual class gtsam::Pose2;
virtual class gtsam::Point3;
virtual class gtsam::Rot3;
virtual class gtsam::Pose3;
class gtsam::Vector6;
class gtsam::LieScalar;
class gtsam::LieVector;
class gtsam::Point2;
class gtsam::Rot2;
class gtsam::Pose2;
class gtsam::Point3;
class gtsam::Rot3;
class gtsam::Pose3;
virtual class gtsam::noiseModel::Base;
virtual class gtsam::noiseModel::Gaussian;
virtual class gtsam::imuBias::ConstantBias;
@ -23,8 +24,8 @@ virtual class gtsam::NoiseModelFactor4;
virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor;
virtual class gtsam::Cal3_S2;
virtual class gtsam::Cal3DS2;
class gtsam::Cal3_S2;
class gtsam::Cal3DS2;
class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph;
class gtsam::Ordering;
@ -48,7 +49,7 @@ class Dummy {
};
#include <gtsam_unstable/dynamics/PoseRTV.h>
virtual class PoseRTV : gtsam::Value {
class PoseRTV {
PoseRTV();
PoseRTV(Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
@ -99,7 +100,7 @@ virtual class PoseRTV : gtsam::Value {
};
#include <gtsam_unstable/geometry/Pose3Upright.h>
virtual class Pose3Upright : gtsam::Value {
class Pose3Upright {
Pose3Upright();
Pose3Upright(const gtsam::Pose3Upright& x);
Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
@ -137,7 +138,7 @@ virtual class Pose3Upright : gtsam::Value {
}; // \class Pose3Upright
#include <gtsam_unstable/geometry/BearingS2.h>
virtual class BearingS2 : gtsam::Value {
class BearingS2 {
BearingS2();
BearingS2(double azimuth, double elevation);
BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);
@ -520,14 +521,14 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
virtual class Reconstruction : gtsam::NonlinearFactor {
Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h);
Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::LieVector& xiK) const;
Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::Vector6& xiK) const;
};
virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor {
DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey,
double h, Matrix Inertia, Vector Fu, double m);
Vector evaluateError(const gtsam::LieVector& xiK, const gtsam::LieVector& xiK_1, const gtsam::Pose3& gK) const;
Vector evaluateError(const gtsam::Vector6& xiK, const gtsam::Vector6& xiK_1, const gtsam::Pose3& gK) const;
};
//*************************************************************************

52
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -64,7 +64,8 @@ public:
}
/// Access via key
VerticalBlockMatrix::Block operator()(Key key) {
FastVector<Key>::const_iterator it = std::find(keys_.begin(),keys_.end(),key);
FastVector<Key>::const_iterator it = std::find(keys_.begin(), keys_.end(),
key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
}
@ -98,7 +99,7 @@ struct CallRecord {
template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) {
jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
jacobians(key).block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
}
/// Handle Leaf Case for Dynamic Matrix type (slower)
template<>
@ -311,9 +312,9 @@ public:
//-----------------------------------------------------------------------------
/// Leaf Expression
template<class T, class Chart=DefaultChart<T> >
template<class T, class Chart = DefaultChart<T> >
class LeafExpression: public ExpressionNode<T> {
typedef ChartValue<T,Chart> value_type; // perhaps this can be something else like a std::pair<T,Chart> ??
typedef ChartValue<T, Chart> value_type; // perhaps this can be something else like a std::pair<T,Chart> ??
/// The key into values
Key key_;
@ -347,8 +348,8 @@ public:
}
/// Construct an execution trace for reverse AD
virtual const value_type& traceExecution(const Values& values, ExecutionTrace<value_type>& trace,
char* raw) const {
virtual const value_type& traceExecution(const Values& values,
ExecutionTrace<value_type>& trace, char* raw) const {
trace.setLeaf(key_);
return dynamic_cast<const value_type&>(values.at(key_));
}
@ -358,7 +359,7 @@ public:
//-----------------------------------------------------------------------------
/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
template<typename T>
class LeafExpression<T, DefaultChart<T> >: public ExpressionNode<T> {
class LeafExpression<T, DefaultChart<T> > : public ExpressionNode<T> {
typedef T value_type;
/// The key into values
@ -405,6 +406,7 @@ public:
// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education.
// to recursively generate a class, that will be the base for function nodes.
//
// The class generated, for three arguments A1, A2, and A3 will be
//
// struct Base1 : Argument<T,A1,1>, FunctionalBase<T> {
@ -429,6 +431,30 @@ public:
//
// All this magic happens when we generate the Base3 base class of FunctionalNode
// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode
//
// Similarly, the inner Record struct will be
//
// struct Record1 : JacobianTrace<T,A1,1>, CallRecord<traits::dimension<T>::value> {
// ... storage related to A1 ...
// ... methods that work on A1 ...
// };
//
// struct Record2 : JacobianTrace<T,A2,2>, Record1 {
// ... storage related to A2 ...
// ... methods that work on A2 and (recursively) on A1 ...
// };
//
// struct Record3 : JacobianTrace<T,A3,3>, Record2 {
// ... storage related to A3 ...
// ... methods that work on A3 and (recursively) on A2 and A1 ...
// };
//
// struct Record : Record3 {
// Provides convenience access to storage in hierarchy by using
// static_cast<JacobianTrace<T, A, N> &>(*this)
// }
//
//-----------------------------------------------------------------------------
/// meta-function to generate fixed-size JacobianTA type
@ -457,6 +483,7 @@ struct FunctionalBase: ExpressionNode<T> {
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record, char*& raw) const {
// base case: does not do anything
}
};
@ -562,15 +589,23 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
template<class T, class TYPES>
struct FunctionalNode {
/// The following typedef generates the recursively defined Base class
typedef typename boost::mpl::fold<TYPES, FunctionalBase<T>,
GenerateFunctionalNode<T, MPL::_2, MPL::_1> >::type Base;
/**
* The type generated by this meta-function derives from Base
* and adds access functions as well as the crucial [trace] function
*/
struct type: public Base {
// Argument types and derived, note these are base 0 !
// These are currently not used - useful for Phoenix in future
#ifdef EXPRESSIONS_PHOENIX
typedef TYPES Arguments;
typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
#endif
/// Reset expression shared pointer
template<class A, size_t N>
@ -725,7 +760,8 @@ public:
typedef boost::function<
T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type, typename OptionalJacobian<T, A3>::type)> Function;
typename OptionalJacobian<T, A2>::type,
typename OptionalJacobian<T, A3>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record;

3
gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <algorithm>
using namespace std;
using namespace gtsam;
@ -143,7 +144,7 @@ TEST(ExpressionFactor, Triple) {
// Test out invoke
TEST(ExpressionFactor, Invoke) {
assert(invoke(add,boost::fusion::make_vector(1,1)) == 2);
EXPECT_LONGS_EQUAL(2, invoke(plus<int>(),boost::fusion::make_vector(1,1)));
// Creating a Pose3 (is there another way?)
boost::fusion::vector<Rot3, Point3> pair;

4
gtsam_unstable/partition/FindSeparator-inl.h
View File

@ -17,12 +17,14 @@
#include <boost/shared_array.hpp>
#include <boost/timer.hpp>
#include "FindSeparator.h"
extern "C" {
#include <metis.h>
#include "metislib.h"
}
#include "FindSeparator.h"
namespace gtsam { namespace partition {

691
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -25,384 +25,405 @@
namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type
* @addtogroup SLAM
*/
template<class VALUE>
class BetweenFactorEM: public NonlinearFactor {
public:
typedef VALUE T;
private:
typedef BetweenFactorEM<VALUE> This;
typedef gtsam::NonlinearFactor Base;
gtsam::Key key1_;
gtsam::Key key2_;
VALUE measured_; /** The measurement */
SharedGaussian model_inlier_;
SharedGaussian model_outlier_;
double prior_inlier_;
double prior_outlier_;
bool flag_bump_up_near_zero_probs_;
/** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<BetweenFactorEM> shared_ptr;
/** default constructor - only use for serialization */
BetweenFactorEM() {
}
/** Constructor */
BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
const double prior_inlier, const double prior_outlier,
const bool flag_bump_up_near_zero_probs = false) :
Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(
measured), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_(
prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(
flag_bump_up_near_zero_probs) {
}
virtual ~BetweenFactorEM() {
}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << ","
<< keyFormatter(key2_) << ")\n";
measured_.print(" measured: ");
model_inlier_->print(" noise model inlier: ");
model_outlier_->print(" noise model outlier: ");
std::cout << "(prior_inlier, prior_outlier_) = (" << prior_inlier_ << ","
<< prior_outlier_ << ")\n";
// Base::print(s, keyFormatter);
}
/** equals */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This *t = dynamic_cast<const This*>(&f);
if (t && Base::equals(f))
return key1_ == t->key1_ && key2_ == t->key2_
&&
// model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
// model_outlier_->equals(t->model_outlier_ ) &&
prior_outlier_ == t->prior_outlier_
&& prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
else
return false;
}
/** implement functions needed to derive from Factor */
/* ************************************************************************* */
virtual double error(const gtsam::Values& x) const {
return whitenedError(x).squaredNorm();
}
/* ************************************************************************* */
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type
* @addtogroup SLAM
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
template<class VALUE>
class BetweenFactorEM: public NonlinearFactor {
/* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
const gtsam::Values& x) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>();
public:
//std::cout<<"About to linearize"<<std::endl;
gtsam::Matrix A1, A2;
std::vector<gtsam::Matrix> A(this->size());
gtsam::Vector b = -whitenedError(x, A);
A1 = A[0];
A2 = A[1];
typedef VALUE T;
return gtsam::GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
gtsam::noiseModel::Unit::Create(b.size())));
}
private:
/* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
typedef BetweenFactorEM<VALUE> This;
typedef gtsam::NonlinearFactor Base;
bool debug = true;
gtsam::Key key1_;
gtsam::Key key2_;
const T& p1 = x.at<T>(key1_);
const T& p2 = x.at<T>(key2_);
VALUE measured_; /** The measurement */
Matrix H1, H2;
SharedGaussian model_inlier_;
SharedGaussian model_outlier_;
T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
double prior_inlier_;
double prior_outlier_;
Vector err = measured_.localCoordinates(hx);
bool flag_bump_up_near_zero_probs_;
// Calculate indicator probabilities (inlier and outlier)
Vector p_inlier_outlier = calcIndicatorProb(x);
double p_inlier = p_inlier_outlier[0];
double p_outlier = p_inlier_outlier[1];
/** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T)
GTSAM_CONCEPT_TESTABLE_TYPE(T)
Vector err_wh_inlier = model_inlier_->whiten(err);
Vector err_wh_outlier = model_outlier_->whiten(err);
public:
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose()
* model_outlier_->R();
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<BetweenFactorEM> shared_ptr;
Vector err_wh_eq;
err_wh_eq.resize(err_wh_inlier.rows() * 2);
err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier)
* err_wh_outlier.array();
/** default constructor - only use for serialization */
BetweenFactorEM() {}
if (H) {
// stack Jacobians for the two indicators for each of the key
/** Constructor */
BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false) :
Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured),
model_inlier_(model_inlier), model_outlier_(model_outlier),
prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs){
Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
(*H)[0].resize(H1_aug.rows(), H1_aug.cols());
(*H)[1].resize(H2_aug.rows(), H2_aug.cols());
(*H)[0] = H1_aug;
(*H)[1] = H2_aug;
}
virtual ~BetweenFactorEM() {}
if (debug) {
// std::cout<<"unwhitened error: "<<err[0]<<" "<<err[1]<<" "<<err[2]<<std::endl;
// std::cout<<"err_wh_inlier: "<<err_wh_inlier[0]<<" "<<err_wh_inlier[1]<<" "<<err_wh_inlier[2]<<std::endl;
// std::cout<<"err_wh_outlier: "<<err_wh_outlier[0]<<" "<<err_wh_outlier[1]<<" "<<err_wh_outlier[2]<<std::endl;
//
// std::cout<<"p_inlier, p_outlier, sumP: "<<p_inlier<<" "<<p_outlier<<" " << sumP << std::endl;
//
// std::cout<<"prior_inlier_, prior_outlier_: "<<prior_inlier_<<" "<<prior_outlier_<< std::endl;
//
// double s_inl = -0.5 * err_wh_inlier.dot(err_wh_inlier);
// double s_outl = -0.5 * err_wh_outlier.dot(err_wh_outlier);
// std::cout<<"s_inl, s_outl: "<<s_inl<<" "<<s_outl<<std::endl;
//
// std::cout<<"norm of invCov_inlier, invCov_outlier: "<<invCov_inlier.norm()<<" "<<invCov_outlier.norm()<<std::endl;
// double q_inl = invCov_inlier.norm() * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
// double q_outl = invCov_outlier.norm() * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
// std::cout<<"q_inl, q_outl: "<<q_inl<<" "<<q_outl<<std::endl;
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BetweenFactorEM("
<< keyFormatter(key1_) << ","
<< keyFormatter(key2_) << ")\n";
measured_.print(" measured: ");
model_inlier_->print(" noise model inlier: ");
model_outlier_->print(" noise model outlier: ");
std::cout << "(prior_inlier, prior_outlier_) = ("
<< prior_inlier_ << ","
<< prior_outlier_ << ")\n";
// Base::print(s, keyFormatter);
// Matrix Cov_inlier = invCov_inlier.inverse();
// Matrix Cov_outlier = invCov_outlier.inverse();
// std::cout<<"Cov_inlier: "<<std::endl<<
// Cov_inlier(0,0) << " " << Cov_inlier(0,1) << " " << Cov_inlier(0,2) <<std::endl<<
// Cov_inlier(1,0) << " " << Cov_inlier(1,1) << " " << Cov_inlier(1,2) <<std::endl<<
// Cov_inlier(2,0) << " " << Cov_inlier(2,1) << " " << Cov_inlier(2,2) <<std::endl;
// std::cout<<"Cov_outlier: "<<std::endl<<
// Cov_outlier(0,0) << " " << Cov_outlier(0,1) << " " << Cov_outlier(0,2) <<std::endl<<
// Cov_outlier(1,0) << " " << Cov_outlier(1,1) << " " << Cov_outlier(1,2) <<std::endl<<
// Cov_outlier(2,0) << " " << Cov_outlier(2,1) << " " << Cov_outlier(2,2) <<std::endl;
// std::cout<<"===="<<std::endl;
}
/** equals */
virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const {
const This *t = dynamic_cast<const This*> (&f);
return err_wh_eq;
}
if(t && Base::equals(f))
return key1_ == t->key1_ && key2_ == t->key2_ &&
// model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
// model_outlier_->equals(t->model_outlier_ ) &&
prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
else
return false;
/* ************************************************************************* */
gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
bool debug = false;
Vector err = unwhitenedError(x);
// Calculate indicator probabilities (inlier and outlier)
Vector err_wh_inlier = model_inlier_->whiten(err);
Vector err_wh_outlier = model_outlier_->whiten(err);
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose()
* model_outlier_->R();
double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant())
* exp(-0.5 * err_wh_inlier.dot(err_wh_inlier));
double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant())
* exp(-0.5 * err_wh_outlier.dot(err_wh_outlier));
if (debug) {
std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", "
<< err[1] << ", " << err[2] << std::endl;
std::cout << "in calcIndicatorProb. err_wh_inlier: " << err_wh_inlier[0]
<< ", " << err_wh_inlier[1] << ", " << err_wh_inlier[2] << std::endl;
std::cout << "in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "
<< err_wh_inlier.dot(err_wh_inlier) << std::endl;
std::cout << "in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "
<< err_wh_outlier.dot(err_wh_outlier) << std::endl;
std::cout
<< "in calcIndicatorProb. p_inlier, p_outlier before normalization: "
<< p_inlier << ", " << p_outlier << std::endl;
}
/** implement functions needed to derive from Factor */
double sumP = p_inlier + p_outlier;
p_inlier /= sumP;
p_outlier /= sumP;
/* ************************************************************************* */
virtual double error(const gtsam::Values& x) const {
return whitenedError(x).squaredNorm();
if (flag_bump_up_near_zero_probs_) {
// Bump up near-zero probabilities (as in linerFlow.h)
double minP = 0.05; // == 0.1 / 2 indicator variables
if (p_inlier < minP || p_outlier < minP) {
if (p_inlier < minP)
p_inlier = minP;
if (p_outlier < minP)
p_outlier = minP;
sumP = p_inlier + p_outlier;
p_inlier /= sumP;
p_outlier /= sumP;
}
}
/* ************************************************************************* */
/**
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
return (Vector(2) << p_inlier, p_outlier);
}
/* ************************************************************************* */
gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
const T& p1 = x.at<T>(key1_);
const T& p2 = x.at<T>(key2_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
return measured_.localCoordinates(hx);
}
/* ************************************************************************* */
void set_flag_bump_up_near_zero_probs(bool flag) {
flag_bump_up_near_zero_probs_ = flag;
}
/* ************************************************************************* */
bool get_flag_bump_up_near_zero_probs() const {
return flag_bump_up_near_zero_probs_;
}
/* ************************************************************************* */
SharedGaussian get_model_inlier() const {
return model_inlier_;
}
/* ************************************************************************* */
SharedGaussian get_model_outlier() const {
return model_outlier_;
}
/* ************************************************************************* */
Matrix get_model_inlier_cov() const {
return (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
}
/* ************************************************************************* */
Matrix get_model_outlier_cov() const {
return (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
}
/* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values,
const gtsam::NonlinearFactorGraph& graph) {
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated).
*
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
*
* TODO: improve efficiency (info form)
*/
/* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>();
//std::cout<<"About to linearize"<<std::endl;
gtsam::Matrix A1, A2;
std::vector<gtsam::Matrix> A(this->size());
gtsam::Vector b = -whitenedError(x, A);
A1 = A[0];
A2 = A[1];
// get joint covariance of the involved states
std::vector<gtsam::Key> Keys;
Keys.push_back(key1_);
Keys.push_back(key2_);
Marginals marginals(graph, values, Marginals::QR);
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
Matrix cov1 = joint_marginal12(key1_, key1_);
Matrix cov2 = joint_marginal12(key2_, key2_);
Matrix cov12 = joint_marginal12(key1_, key2_);
return gtsam::GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size())));
}
updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
}
/* ************************************************************************* */
void updateNoiseModels_givenCovs(const gtsam::Values& values,
const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated).
*
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
*
* TODO: improve efficiency (info form)
*/
/* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
const T& p1 = values.at<T>(key1_);
const T& p2 = values.at<T>(key2_);
bool debug = true;
Matrix H1, H2;
p1.between(p2, H1, H2); // h(x)
const T& p1 = x.at<T>(key1_);
const T& p2 = x.at<T>(key2_);
Matrix H;
H.resize(H1.rows(), H1.rows() + H2.rows());
H << H1, H2; // H = [H1 H2]
Matrix H1, H2;
Matrix joint_cov;
joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols());
joint_cov << cov1, cov12, cov12.transpose(), cov2;
T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
Matrix cov_state = H * joint_cov * H.transpose();
Vector err = measured_.localCoordinates(hx);
// model_inlier_->print("before:");
// Calculate indicator probabilities (inlier and outlier)
Vector p_inlier_outlier = calcIndicatorProb(x);
double p_inlier = p_inlier_outlier[0];
double p_outlier = p_inlier_outlier[1];
// update inlier and outlier noise models
Matrix covRinlier =
(model_inlier_->R().transpose() * model_inlier_->R()).inverse();
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(
covRinlier + cov_state);
Vector err_wh_inlier = model_inlier_->whiten(err);
Vector err_wh_outlier = model_outlier_->whiten(err);
Matrix covRoutlier =
(model_outlier_->R().transpose() * model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(
covRoutlier + cov_state);
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
// model_inlier_->print("after:");
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
}
Vector err_wh_eq;
err_wh_eq.resize(err_wh_inlier.rows()*2);
err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array();
/* ************************************************************************* */
/** return the measured */
const VALUE& measured() const {
return measured_;
}
if (H){
// stack Jacobians for the two indicators for each of the key
/** number of variables attached to this factor */
std::size_t size() const {
return 2;
}
Matrix H1_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H1);
Matrix H1_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H1);
Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
virtual size_t dim() const {
return model_inlier_->R().rows() + model_inlier_->R().cols();
}
Matrix H2_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H2);
Matrix H2_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H2);
Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
private:
(*H)[0].resize(H1_aug.rows(),H1_aug.cols());
(*H)[1].resize(H2_aug.rows(),H2_aug.cols());
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
// \class BetweenFactorEM
(*H)[0] = H1_aug;
(*H)[1] = H2_aug;
}
if (debug){
// std::cout<<"unwhitened error: "<<err[0]<<" "<<err[1]<<" "<<err[2]<<std::endl;
// std::cout<<"err_wh_inlier: "<<err_wh_inlier[0]<<" "<<err_wh_inlier[1]<<" "<<err_wh_inlier[2]<<std::endl;
// std::cout<<"err_wh_outlier: "<<err_wh_outlier[0]<<" "<<err_wh_outlier[1]<<" "<<err_wh_outlier[2]<<std::endl;
//
// std::cout<<"p_inlier, p_outlier, sumP: "<<p_inlier<<" "<<p_outlier<<" " << sumP << std::endl;
//
// std::cout<<"prior_inlier_, prior_outlier_: "<<prior_inlier_<<" "<<prior_outlier_<< std::endl;
//
// double s_inl = -0.5 * err_wh_inlier.dot(err_wh_inlier);
// double s_outl = -0.5 * err_wh_outlier.dot(err_wh_outlier);
// std::cout<<"s_inl, s_outl: "<<s_inl<<" "<<s_outl<<std::endl;
//
// std::cout<<"norm of invCov_inlier, invCov_outlier: "<<invCov_inlier.norm()<<" "<<invCov_outlier.norm()<<std::endl;
// double q_inl = invCov_inlier.norm() * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
// double q_outl = invCov_outlier.norm() * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
// std::cout<<"q_inl, q_outl: "<<q_inl<<" "<<q_outl<<std::endl;
// Matrix Cov_inlier = invCov_inlier.inverse();
// Matrix Cov_outlier = invCov_outlier.inverse();
// std::cout<<"Cov_inlier: "<<std::endl<<
// Cov_inlier(0,0) << " " << Cov_inlier(0,1) << " " << Cov_inlier(0,2) <<std::endl<<
// Cov_inlier(1,0) << " " << Cov_inlier(1,1) << " " << Cov_inlier(1,2) <<std::endl<<
// Cov_inlier(2,0) << " " << Cov_inlier(2,1) << " " << Cov_inlier(2,2) <<std::endl;
// std::cout<<"Cov_outlier: "<<std::endl<<
// Cov_outlier(0,0) << " " << Cov_outlier(0,1) << " " << Cov_outlier(0,2) <<std::endl<<
// Cov_outlier(1,0) << " " << Cov_outlier(1,1) << " " << Cov_outlier(1,2) <<std::endl<<
// Cov_outlier(2,0) << " " << Cov_outlier(2,1) << " " << Cov_outlier(2,2) <<std::endl;
// std::cout<<"===="<<std::endl;
}
return err_wh_eq;
}
/* ************************************************************************* */
gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
bool debug = false;
Vector err = unwhitenedError(x);
// Calculate indicator probabilities (inlier and outlier)
Vector err_wh_inlier = model_inlier_->whiten(err);
Vector err_wh_outlier = model_outlier_->whiten(err);
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
if (debug){
std::cout<<"in calcIndicatorProb. err_unwh: "<<err[0]<<", "<<err[1]<<", "<<err[2]<<std::endl;
std::cout<<"in calcIndicatorProb. err_wh_inlier: "<<err_wh_inlier[0]<<", "<<err_wh_inlier[1]<<", "<<err_wh_inlier[2]<<std::endl;
std::cout<<"in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "<<err_wh_inlier.dot(err_wh_inlier)<<std::endl;
std::cout<<"in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "<<err_wh_outlier.dot(err_wh_outlier)<<std::endl;
std::cout<<"in calcIndicatorProb. p_inlier, p_outlier before normalization: "<<p_inlier<<", "<<p_outlier<<std::endl;
}
double sumP = p_inlier + p_outlier;
p_inlier /= sumP;
p_outlier /= sumP;
if (flag_bump_up_near_zero_probs_){
// Bump up near-zero probabilities (as in linerFlow.h)
double minP = 0.05; // == 0.1 / 2 indicator variables
if (p_inlier < minP || p_outlier < minP){
if (p_inlier < minP)
p_inlier = minP;
if (p_outlier < minP)
p_outlier = minP;
sumP = p_inlier + p_outlier;
p_inlier /= sumP;
p_outlier /= sumP;
}
}
return (Vector(2) << p_inlier, p_outlier);
}
/* ************************************************************************* */
gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
const T& p1 = x.at<T>(key1_);
const T& p2 = x.at<T>(key2_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
return measured_.localCoordinates(hx);
}
/* ************************************************************************* */
void set_flag_bump_up_near_zero_probs(bool flag) {
flag_bump_up_near_zero_probs_ = flag;
}
/* ************************************************************************* */
bool get_flag_bump_up_near_zero_probs() const {
return flag_bump_up_near_zero_probs_;
}
/* ************************************************************************* */
SharedGaussian get_model_inlier() const {
return model_inlier_;
}
/* ************************************************************************* */
SharedGaussian get_model_outlier() const {
return model_outlier_;
}
/* ************************************************************************* */
Matrix get_model_inlier_cov() const {
return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
}
/* ************************************************************************* */
Matrix get_model_outlier_cov() const {
return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
}
/* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated).
*
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
*
* TODO: improve efficiency (info form)
*/
// get joint covariance of the involved states
std::vector<gtsam::Key> Keys;
Keys.push_back(key1_);
Keys.push_back(key2_);
Marginals marginals( graph, values, Marginals::QR );
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
Matrix cov1 = joint_marginal12(key1_, key1_);
Matrix cov2 = joint_marginal12(key2_, key2_);
Matrix cov12 = joint_marginal12(key1_, key2_);
updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
}
/* ************************************************************************* */
void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated).
*
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
*
* TODO: improve efficiency (info form)
*/
const T& p1 = values.at<T>(key1_);
const T& p2 = values.at<T>(key2_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows());
H << H1, H2; // H = [H1 H2]
Matrix joint_cov;
joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
joint_cov << cov1, cov12,
cov12.transpose(), cov2;
Matrix cov_state = H*joint_cov*H.transpose();
// model_inlier_->print("before:");
// update inlier and outlier noise models
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
// model_inlier_->print("after:");
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
}
/* ************************************************************************* */
/** return the measured */
const VALUE& measured() const {
return measured_;
}
/** number of variables attached to this factor */
std::size_t size() const {
return 2;
}
virtual size_t dim() const {
return model_inlier_->R().rows() + model_inlier_->R().cols();
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // \class BetweenFactorEM
} /// namespace gtsam
}/// namespace gtsam

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -372,7 +372,7 @@ namespace gtsam {
const T& p2 = values.at<T>(keyB_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
p1.between(p2, H1, H2); // h(x)
Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows());

5
gtsam_unstable/timing/timeDSFvariants.cpp
View File

@ -57,7 +57,8 @@ int main(int argc, char* argv[]) {
volatile double fpm = 0.5; // fraction of points matched
volatile size_t nm = fpm * n * np; // number of matches
cout << format("\nTesting with %1% images, %2% points, %3% matches\n") % m % N % nm;
cout << format("\nTesting with %1% images, %2% points, %3% matches\n")
% (int)m % (int)N % (int)nm;
cout << "Generating " << nm << " matches" << endl;
boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rn(
boost::mt19937(), boost::uniform_int<size_t>(0, N - 1));
@ -67,7 +68,7 @@ int main(int argc, char* argv[]) {
for (size_t k = 0; k < nm; k++)
matches.push_back(Match(rn(), rn()));
os << format("%1%,%2%,%3%,") % m % N % nm;
os << format("%1%,%2%,%3%,") % (int)m % (int)N % (int)nm;
{
// DSFBase version

12
gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -35,12 +35,12 @@ gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
/* ************************************************************************* */
gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) {
gtsam::Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6));
}
gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) {
return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3));
gtsam::Vector3 predictionErrorVel(const Pose3& p1, const Vector3& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
return factor.evaluateError(p1, v1, b1, p2, v2).tail(3);
}
#include <gtsam/linear/GaussianFactorGraph.h>
@ -64,21 +64,21 @@ int main() {
Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
0.580273724, 0.693095498, -0.427669306,
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
LieVector Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4));
Vector3 Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
LieVector Vel2 = Vel1.compose( dv );
Vector3 Vel2 = Vel1 + dv;
imuBias::ConstantBias Bias1;
Values values;

8
gtsam_unstable/timing/timeSFMExpressions.cpp
View File

@ -47,15 +47,15 @@ int main() {
// Create values
Values values;
values.insert(Symbol('K', 0), Cal3_S2());
for (int i = 0; i < M; i++)
for (size_t i = 0; i < M; i++)
values.insert(Symbol('x', i), Pose3());
for (int j = 0; j < N; j++)
for (size_t j = 0; j < N; j++)
values.insert(Symbol('p', j), Point3(0, 0, 1));
long timeLog = clock();
NonlinearFactorGraph graph;
for (int i = 0; i < M; i++) {
for (int j = 0; j < N; j++) {
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
NonlinearFactor::shared_ptr f = boost::make_shared<
ExpressionFactor<Point2> >
#ifdef TERNARY

2
matlab/+gtsam/VisualISAMPlot.m
View File

@ -16,7 +16,7 @@ gtsam.plot3DPoints(result, [], marginals);
M = 1;
while result.exists(symbol('x',M))
ii = symbol('x',M);
pose_i = result.at(ii);
pose_i = result.atPose3(ii);
if options.hardConstraint && (M==1)
gtsam.plotPose3(pose_i,[],10);
else

2
matlab/+gtsam/VisualISAMStep.m
View File

@ -27,7 +27,7 @@ for k=1:length(data.Z{nextPoseIndex})
end
%% Initial estimates for the new pose.
prevPose = result.at(symbol('x',nextPoseIndex-1));
prevPose = result.atPose3(symbol('x',nextPoseIndex-1));
initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry));
%% Update ISAM

4
matlab/+gtsam/load3D.m
View File

@ -46,9 +46,9 @@ for i=1:n
graph.add(BetweenFactorPose3(i1, i2, dpose, model));
if successive
if i2>i1
initial.insert(i2,initial.at(i1).compose(dpose));
initial.insert(i2,initial.atPose3(i1).compose(dpose));
else
initial.insert(i1,initial.at(i2).compose(dpose.inverse));
initial.insert(i1,initial.atPose3(i2).compose(dpose.inverse));
end
end
end

7
matlab/+gtsam/plot2DPoints.m
View File

@ -18,15 +18,18 @@ hold on
% Plot points and covariance matrices
for i = 0:keys.size-1
key = keys.at(i);
p = values.at(key);
if isa(p, 'gtsam.Point2')
try
p = values.atPoint2(key);
if haveMarginals
P = marginals.marginalCovariance(key);
gtsam.plotPoint2(p, linespec, P);
else
gtsam.plotPoint2(p, linespec);
end
catch err
% I guess it's not a Point2
end
end
if ~holdstate

30
matlab/+gtsam/plot2DTrajectory.m
View File

@ -8,7 +8,7 @@ function plot2DTrajectory(values, linespec, marginals)
import gtsam.*
if ~exist('linespec', 'var') || isempty(linespec)
linespec = 'k*-';
linespec = 'k*-';
end
haveMarginals = exist('marginals', 'var');
@ -25,24 +25,26 @@ plot(X,Y,linespec);
% Quiver can also be vectorized if no marginals asked
if ~haveMarginals
C = cos(theta);
S = sin(theta);
quiver(X,Y,C,S,0.1,linespec);
C = cos(theta);
S = sin(theta);
quiver(X,Y,C,S,0.1,linespec);
else
% plotPose2 does both quiver and covariance matrix
keys = KeyVector(values.keys);
for i = 0:keys.size-1
key = keys.at(i);
x = values.at(key);
if isa(x, 'gtsam.Pose2')
P = marginals.marginalCovariance(key);
gtsam.plotPose2(x,linespec(1), P);
% plotPose2 does both quiver and covariance matrix
keys = KeyVector(values.keys);
for i = 0:keys.size-1
key = keys.at(i);
try
x = values.atPose2(key);
P = marginals.marginalCovariance(key);
gtsam.plotPose2(x,linespec(1), P);
catch err
% I guess it's not a Pose2
end
end
end
end
if ~holdstate
hold off
hold off
end
end

6
matlab/+gtsam/plot3DPoints.m
View File

@ -18,14 +18,16 @@ hold on
% Plot points and covariance matrices
for i = 0:keys.size-1
key = keys.at(i);
p = values.at(key);
if isa(p, 'gtsam.Point3')
try
p = values.atPoint3(key);
if haveMarginals
P = marginals.marginalCovariance(key);
gtsam.plotPoint3(p, linespec, P);
else
gtsam.plotPoint3(p, linespec);
end
catch
% I guess it's not a Point3
end
end

53
matlab/+gtsam/plot3DTrajectory.m
View File

@ -17,39 +17,48 @@ hold on
lastIndex = [];
for i = 0:keys.size-1
key = keys.at(i);
x = values.at(key);
if isa(x, 'gtsam.Pose3')
try
x = values.atPose3(key);
if ~isempty(lastIndex)
% Draw line from last pose then covariance ellipse on top of
% last pose.
lastKey = keys.at(lastIndex);
lastPose = values.at(lastKey);
plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
try
lastPose = values.atPose3(lastKey);
plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
else
P = [];
end
gtsam.plotPose3(lastPose, P, scale);
catch err
warning(['no Pose3 at ' lastKey]);
end
lastIndex = i;
end
catch
warning(['no Pose3 at ' key]);
end
% Draw final pose
if ~isempty(lastIndex)
lastKey = keys.at(lastIndex);
try
lastPose = values.atPose3(lastKey);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
else
P = [];
end
gtsam.plotPose3(lastPose, P, scale);
catch
warning(['no Pose3 at ' lastIndex]);
end
lastIndex = i;
end
end
% Draw final pose
if ~isempty(lastIndex)
lastKey = keys.at(lastIndex);
lastPose = values.at(lastKey);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
else
P = [];
if ~holdstate
hold off
end
gtsam.plotPose3(lastPose, P, scale);
end
if ~holdstate
hold off
end
end

6
matlab/gtsam_examples/PlanarSLAMExample.m
View File

@ -71,9 +71,9 @@ marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals);
plot2DPoints(result, 'b', marginals);
plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-');
plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');
plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-');
plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-');
axis([-0.6 4.8 -1 1])
axis equal
view(2)

2
matlab/gtsam_examples/PlanarSLAMExample_graph.m
View File

@ -22,7 +22,7 @@ model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);
[graph,initial] = load2D(datafile, model);
%% Add a Gaussian prior on a pose in the middle
priorMean = initial.at(40);
priorMean = initial.atPose2(40);
priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]);
graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph

8
matlab/gtsam_examples/PlanarSLAMExample_sampling.m
View File

@ -55,14 +55,14 @@ plot2DPoints(sample, [], marginals);
for j=1:2
key = symbol('l',j);
point{j} = sample.at(key);
point{j} = sample.atPoint2(key);
Q{j}=marginals.marginalCovariance(key);
S{j}=chol(Q{j}); % for sampling
end
plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-');
plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-');
plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-');
plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-');
view(2); axis auto; axis equal
%% Do Sampling on point 2

2
matlab/gtsam_examples/Pose2SLAMExample.m
View File

@ -57,7 +57,7 @@ result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses
cla;
hold on
plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-');
plot([result.atPose2(5).x;result.atPose2(2).x],[result.atPose2(5).y;result.atPose2(2).y],'r-');
marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals);

14
matlab/gtsam_examples/Pose2SLAMExample_circle.m
View File

@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose2(6,1.0);
p0 = hexagon.at(0);
p1 = hexagon.at(1);
p0 = hexagon.atPose2(0);
p1 = hexagon.atPose2(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@ -32,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance));
%% Create initial config
initial = Values;
initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]'));
initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]'));
initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]'));
initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]'));
initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]'));
initial.insert(1, hexagon.atPose2(1).retract([-0.1, 0.1,-0.1]'));
initial.insert(2, hexagon.atPose2(2).retract([ 0.1,-0.1, 0.1]'));
initial.insert(3, hexagon.atPose2(3).retract([-0.1, 0.1,-0.1]'));
initial.insert(4, hexagon.atPose2(4).retract([ 0.1,-0.1, 0.1]'));
initial.insert(5, hexagon.atPose2(5).retract([-0.1, 0.1,-0.1]'));
%% Plot Initial Estimate
cla

2
matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

@ -41,7 +41,7 @@ marginals = Marginals(graph, result);
toc
P={};
for i=1:result.size()-1
pose_i = result.at(i);
pose_i = result.atPose2(i);
P{i}=marginals.marginalCovariance(i);
plotPose2(pose_i,'b',P{i})
end

14
matlab/gtsam_examples/Pose3SLAMExample.m
View File

@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose3(6,1.0);
p0 = hexagon.at(0);
p1 = hexagon.at(1);
p0 = hexagon.atPose3(0);
p1 = hexagon.atPose3(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@ -33,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values;
s = 0.10;
initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
%% Plot Initial Estimate
cla

2
matlab/gtsam_examples/Pose3SLAMExample_graph.m
View File

@ -28,7 +28,7 @@ model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.
%% Plot Initial Estimate
cla
first = initial.at(0);
first = initial.atPose3(0);
plot3(first.x(),first.y(),first.z(),'r*'); hold on
plot3DTrajectory(initial,'g-',false);
drawnow;

4
matlab/gtsam_examples/StereoVOExample_large.m
View File

@ -45,7 +45,7 @@ for i=1:size(measurements,1)
if ~initial.exists(symbol('l',sf(2)))
% 3D landmarks are stored in camera coordinates: transform
% to world coordinates using the respective initial pose
pose = initial.at(symbol('x', sf(1)));
pose = initial.atPose3(symbol('x', sf(1)));
world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8)));
initial.insert(symbol('l',sf(2)), world_point);
end
@ -54,7 +54,7 @@ toc
%% add a constraint on the starting pose
key = symbol('x',1);
first_pose = initial.at(key);
first_pose = initial.atPose3(key);
graph.add(NonlinearEqualityPose3(key, first_pose));
%% optimize

6
matlab/gtsam_tests/testLocalizationExample.m
View File

@ -29,7 +29,7 @@ groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
for i=1:3
graph.add(PriorFactorPose2(i, groundTruth.at(i), model));
graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
end
%% Initialize to noisy points
@ -46,7 +46,7 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
P={};
for i=1:result.size()
pose_i = result.at(i);
CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4));
pose_i = result.atPose2(i);
CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4));
P{i}=marginals.marginalCovariance(i);
end

2
matlab/gtsam_tests/testOdometryExample.m
View File

@ -39,5 +39,5 @@ marginals = Marginals(graph, result);
marginals.marginalCovariance(1);
%% Check first pose equality
pose_1 = result.at(1);
pose_1 = result.atPose2(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

4
matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -60,10 +60,10 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
%% Check first pose and point equality
pose_1 = result.at(symbol('x',1));
pose_1 = result.atPose2(symbol('x',1));
marginals.marginalCovariance(symbol('x',1));
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
point_1 = result.at(symbol('l',1));
point_1 = result.atPoint2(symbol('l',1));
marginals.marginalCovariance(symbol('l',1));
CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));

2
matlab/gtsam_tests/testPose2SLAMExample.m
View File

@ -57,6 +57,6 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
P = marginals.marginalCovariance(1);
pose_1 = result.at(1);
pose_1 = result.atPose2(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

16
matlab/gtsam_tests/testPose3SLAMExample.m
View File

@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose3(6,1.0);
p0 = hexagon.at(0);
p1 = hexagon.at(1);
p0 = hexagon.atPose3(0);
p1 = hexagon.atPose3(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@ -33,17 +33,17 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values;
s = 0.10;
initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
%% optimize
optimizer = LevenbergMarquardtOptimizer(fg, initial);
result = optimizer.optimizeSafely;
pose_1 = result.at(1);
pose_1 = result.atPose3(1);
CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4));

4
matlab/gtsam_tests/testSFMExample.m
View File

@ -63,11 +63,11 @@ marginals.marginalCovariance(symbol('x',1));
%% Check optimized results, should be equal to ground truth
for i=1:size(truth.cameras,2)
pose_i = result.at(symbol('x',i));
pose_i = result.atPose3(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
point_j = result.at(symbol('p',j));
point_j = result.atPoint3(symbol('p',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end

4
matlab/gtsam_tests/testStereoVOExample.m
View File

@ -61,8 +61,8 @@ optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize();
%% check equality for the first pose and point
pose_x1 = result.at(x1);
pose_x1 = result.atPose3(x1);
CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
point_l1 = result.at(l1);
point_l1 = result.atPoint3(l1);
CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));

4
matlab/gtsam_tests/testVisualISAMExample.m
View File

@ -45,11 +45,11 @@ for frame_i=3:options.nrCameras
end
for i=1:size(truth.cameras,2)
pose_i = result.at(symbol('x',i));
pose_i = result.atPose3(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
point_j = result.at(symbol('l',j));
point_j = result.atPoint3(symbol('l',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end

6
matlab/gtsam_tests/test_gtsam.m
View File

@ -30,11 +30,11 @@ testStereoVOExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
display 'Starting: testSerialization'
testSerialization
display 'Starting: testUtilities'
testUtilities
display 'Starting: testSerialization'
testSerialization
% end of tests
display 'Tests complete!'

2
package_scripts/toolbox_package_unix.sh
View File

@ -61,4 +61,4 @@ if [ $? -ne 0 ]; then
fi
# Create package
tar czf gtsam-toolbox-3.0.0-$platform.tgz -C stage/gtsam_toolbox toolbox
tar czf gtsam-toolbox-3.2.0-$platform.tgz -C stage/gtsam_toolbox toolbox

5
tests/testPCGSolver.cpp
View File

@ -13,6 +13,7 @@
* @file testPCGSolver.cpp
* @brief Unit tests for PCGSolver class
* @author Yong-Dian Jian
* @date Aug 06, 2014
*/
#include <tests/smallExample.h>
@ -51,6 +52,7 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
// Test cholesky decomposition
TEST( PCGSolver, llt ) {
Matrix R = (Matrix(3,3) <<
1., -1., -1.,
@ -90,6 +92,7 @@ TEST( PCGSolver, llt ) {
}
/* ************************************************************************* */
// Test Dummy Preconditioner
TEST( PCGSolver, dummy )
{
LevenbergMarquardtParams paramsPCG;
@ -110,6 +113,7 @@ TEST( PCGSolver, dummy )
}
/* ************************************************************************* */
// Test Block-Jacobi Precondioner
TEST( PCGSolver, blockjacobi )
{
LevenbergMarquardtParams paramsPCG;
@ -130,6 +134,7 @@ TEST( PCGSolver, blockjacobi )
}
/* ************************************************************************* */
// Test Incremental Subgraph PCG Solver
TEST( PCGSolver, subgraph )
{
LevenbergMarquardtParams paramsPCG;

115
tests/testPreconditioner.cpp Normal file
View File

@ -0,0 +1,115 @@
/* ----------------------------------------------------------------------------
* 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 testPreconditioner.cpp
* @brief Unit tests for Preconditioners
* @author Sungtae An
* @date Nov 6, 2014
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/linear/PCGSolver.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
static GaussianFactorGraph createSimpleGaussianFactorGraph() {
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);
return fg;
}
/* ************************************************************************* */
// Copy of BlockJacobiPreconditioner::build
std::vector<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo)
{
const size_t n = keyInfo.size();
std::vector<size_t> dims_ = keyInfo.colSpec();
/* prepare the buffer of block diagonals */
std::vector<Matrix> blocks; blocks.reserve(n);
/* allocate memory for the factorization of block diagonals */
size_t nnz = 0;
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t dim = dims_[i];
blocks.push_back(Matrix::Zero(dim, dim));
// nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
nnz += dim*dim;
}
/* compute the block diagonal by scanning over the factors */
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Ai = jf->getA(it);
blocks[entry.index()] += (Ai.transpose() * Ai);
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Hii = hf->info(it, it).selfadjointView();
blocks[entry.index()] += Hii;
}
}
else {
throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
return blocks;
}
/* ************************************************************************* */
TEST( Preconditioner, buildBlocks ) {
// Create simple Gaussian factor graph and initial values
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
Values initial;
initial.insert(0,Point2(4, 5));
initial.insert(1,Point2(0, 1));
initial.insert(2,Point2(-5, 7));
// Expected Hessian block diagonal matrices
std::map<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
// Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
std::vector<Matrix> actualHessian = buildBlocks(gfg, KeyInfo(gfg));
// Compare the number of block diagonal matrices
EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size());
// Compare the values of matrices
std::map<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
std::vector<Matrix>::const_iterator it2 = actualHessian.begin();
for(; it1!=expectedHessian.end(); it1++, it2++)
EXPECT(assert_equal(it1->second, *it2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

6
timing/timeMatrixOps.cpp
View File

@ -62,9 +62,9 @@ int main(int argc, char* argv[]) {
gtsam::SubMatrix top = mat.block(0, 0, n, n);
gtsam::SubMatrix block = mat.block(m/4, n/4, m-m/2, n-n/2);
cout << format(" Basic: %1%x%2%\n") % m % n;
cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n;
cout << format(" Top: mat(%1%:%2%, %3%:%4%)\n") % 0 % n % 0 % n;
cout << format(" Basic: %1%x%2%\n") % (int)m % (int)n;
cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)m % 0 % (int)n;
cout << format(" Top: mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)n % 0 % (int)n;
cout << format(" Block: mat(%1%:%2%, %3%:%4%)\n") % size_t(m/4) % size_t(m-m/4) % size_t(n/4) % size_t(n-n/4);
cout << endl;

2
timing/timePinholeCamera.cpp
View File

@ -89,7 +89,7 @@ int main()
Matrix Dpose, Dpoint;
long timeLog = clock();
for(int i = 0; i < n; i++)
camera.project(point1, Dpose, Dpoint);
camera.project(point1, Dpose, Dpoint, boost::none);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;

91
wrap/Argument.cpp
View File

@ -28,34 +28,53 @@
using namespace std;
using namespace wrap;
/* ************************************************************************* */
Argument Argument::expandTemplate(const TemplateSubstitution& ts) const {
Argument instArg = *this;
instArg.type = ts(type);
return instArg;
}
/* ************************************************************************* */
ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const {
ArgumentList instArgList;
BOOST_FOREACH(const Argument& arg, *this) {
Argument instArg = arg.expandTemplate(ts);
instArgList.push_back(instArg);
}
return instArgList;
}
/* ************************************************************************* */
string Argument::matlabClass(const string& delim) const {
string result;
BOOST_FOREACH(const string& ns, namespaces)
BOOST_FOREACH(const string& ns, type.namespaces)
result += ns + delim;
if (type == "string" || type == "unsigned char" || type == "char")
if (type.name == "string" || type.name == "unsigned char"
|| type.name == "char")
return result + "char";
if (type == "Vector" || type == "Matrix")
if (type.name == "Vector" || type.name == "Matrix")
return result + "double";
if (type == "int" || type == "size_t")
if (type.name == "int" || type.name == "size_t")
return result + "numeric";
if (type == "bool")
if (type.name == "bool")
return result + "logical";
return result + type;
return result + type.name;
}
/* ************************************************************************* */
bool Argument::isScalar() const {
return (type == "bool" || type == "char" || type == "unsigned char"
|| type == "int" || type == "size_t" || type == "double");
return (type.name == "bool" || type.name == "char"
|| type.name == "unsigned char" || type.name == "int"
|| type.name == "size_t" || type.name == "double");
}
/* ************************************************************************* */
void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << " ";
string cppType = qualifiedType("::");
string matlabUniqueType = qualifiedType();
string cppType = type.qualifiedName("::");
string matlabUniqueType = type.qualifiedName();
if (is_ptr)
// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
@ -78,14 +97,6 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << ");" << endl;
}
/* ************************************************************************* */
string Argument::qualifiedType(const string& delim) const {
string result;
BOOST_FOREACH(const string& ns, namespaces)
result += ns + delim;
return result + type;
}
/* ************************************************************************* */
string ArgumentList::types() const {
string str;
@ -93,7 +104,7 @@ string ArgumentList::types() const {
BOOST_FOREACH(Argument arg, *this) {
if (!first)
str += ",";
str += arg.type;
str += arg.type.name;
first = false;
}
return str;
@ -105,14 +116,14 @@ string ArgumentList::signature() const {
bool cap = false;
BOOST_FOREACH(Argument arg, *this) {
BOOST_FOREACH(char ch, arg.type)
BOOST_FOREACH(char ch, arg.type.name)
if (isupper(ch)) {
sig += ch;
//If there is a capital letter, we don't want to read it below
cap = true;
}
if (!cap)
sig += arg.type[0];
sig += arg.type.name[0];
//Reset to default
cap = false;
}
@ -136,7 +147,8 @@ string ArgumentList::names() const {
/* ************************************************************************* */
bool ArgumentList::allScalar() const {
BOOST_FOREACH(Argument arg, *this)
if (!arg.isScalar()) return false;
if (!arg.isScalar())
return false;
return true;
}
@ -158,42 +170,43 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const {
BOOST_FOREACH(Argument arg, *this) {
if (!first)
file.oss << ", ";
file.oss << arg.type << " " << arg.name;
file.oss << arg.type.name << " " << arg.name;
first = false;
}
file.oss << ")";
}
/* ************************************************************************* */
void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal,
const string& wrapperName, int id, bool staticMethod) const {
returnVal.emit_matlab(file);
file.oss << wrapperName << "(" << id;
void ArgumentList::emit_call(FileWriter& proxyFile,
const ReturnValue& returnVal, const string& wrapperName, int id,
bool staticMethod) const {
returnVal.emit_matlab(proxyFile);
proxyFile.oss << wrapperName << "(" << id;
if (!staticMethod)
file.oss << ", this";
file.oss << ", varargin{:});\n";
proxyFile.oss << ", this";
proxyFile.oss << ", varargin{:});\n";
}
/* ************************************************************************* */
void ArgumentList::emit_conditional_call(FileWriter& file,
void ArgumentList::emit_conditional_call(FileWriter& proxyFile,
const ReturnValue& returnVal, const string& wrapperName, int id,
bool staticMethod) const {
// Check nr of arguments
file.oss << "if length(varargin) == " << size();
proxyFile.oss << "if length(varargin) == " << size();
if (size() > 0)
file.oss << " && ";
// ...and their types
proxyFile.oss << " && ";
// ...and their type.names
bool first = true;
for (size_t i = 0; i < size(); i++) {
if (!first)
file.oss << " && ";
file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".")
<< "')";
proxyFile.oss << " && ";
proxyFile.oss << "isa(varargin{" << i + 1 << "},'"
<< (*this)[i].matlabClass(".") << "')";
first = false;
}
file.oss << "\n";
proxyFile.oss << "\n";
// output call to C++ wrapper
file.oss << " ";
emit_call(file, returnVal, wrapperName, id, staticMethod);
proxyFile.oss << " ";
emit_call(proxyFile, returnVal, wrapperName, id, staticMethod);
}
/* ************************************************************************* */

49
wrap/Argument.h
View File

@ -19,36 +19,39 @@
#pragma once
#include "TemplateSubstitution.h"
#include "FileWriter.h"
#include "ReturnValue.h"
#include <string>
#include <vector>
namespace wrap {
/// Argument class
struct Argument {
Qualified type;
bool is_const, is_ref, is_ptr;
std::string type;
std::string name;
std::vector<std::string> namespaces;
Argument() :
is_const(false), is_ref(false), is_ptr(false) {
}
Argument expandTemplate(const TemplateSubstitution& ts) const;
/// return MATLAB class for use in isa(x,class)
std::string matlabClass(const std::string& delim = "") const;
/// Check if will be unwrapped using scalar login in wrap/matlab.h
bool isScalar() const;
/// adds namespaces to type
std::string qualifiedType(const std::string& delim = "") const;
/// MATLAB code generation, MATLAB to C++
void matlab_unwrap(FileWriter& file, const std::string& matlabName) const;
friend std::ostream& operator<<(std::ostream& os, const Argument& arg) {
os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "")
<< (arg.is_ref ? "&" : "");
return os;
}
};
/// Argument list is just a container with Arguments
@ -66,6 +69,8 @@ struct ArgumentList: public std::vector<Argument> {
/// Check if all arguments scalar
bool allScalar() const;
ArgumentList expandTemplate(const TemplateSubstitution& ts) const;
// MATLAB code generation:
/**
@ -83,24 +88,38 @@ struct ArgumentList: public std::vector<Argument> {
void emit_prototype(FileWriter& file, const std::string& name) const;
/**
* emit emit MATLAB call to wrapper
* @param file output stream
* emit emit MATLAB call to proxy
* @param proxyFile output stream
* @param returnVal the return value
* @param wrapperName of method or function
* @param staticMethod flag to emit "this" in call
*/
void emit_call(FileWriter& file, const ReturnValue& returnVal,
void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal,
const std::string& wrapperName, int id, bool staticMethod = false) const;
/**
* emit conditional MATLAB call to wrapper (checking arguments first)
* @param file output stream
* emit conditional MATLAB call to proxy (checking arguments first)
* @param proxyFile output stream
* @param returnVal the return value
* @param wrapperName of method or function
* @param staticMethod flag to emit "this" in call
*/
void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal,
const std::string& wrapperName, int id, bool staticMethod = false) const;
void emit_conditional_call(FileWriter& proxyFile,
const ReturnValue& returnVal, const std::string& wrapperName, int id,
bool staticMethod = false) const;
friend std::ostream& operator<<(std::ostream& os,
const ArgumentList& argList) {
os << "(";
if (argList.size() > 0)
os << argList.front();
if (argList.size() > 1)
for (size_t i = 1; i < argList.size(); i++)
os << ", " << argList[i];
os << ")";
return os;
}
};
} // \namespace wrap

316
wrap/Class.cpp
View File

@ -33,7 +33,7 @@ using namespace std;
using namespace wrap;
/* ************************************************************************* */
void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
vector<string>& functionNames) const {
@ -41,17 +41,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
createNamespaceStructure(namespaces, toolboxPath);
// open destination classFile
string classFile = toolboxPath;
if (!namespaces.empty())
classFile += "/+" + wrap::qualifiedName("/+", namespaces);
classFile += "/" + name + ".m";
string classFile = matlabName(toolboxPath);
FileWriter proxyFile(classFile, verbose_, "%");
// get the name of actual matlab object
const string matlabQualName = qualifiedName("."), matlabUniqueName =
qualifiedName(), cppName = qualifiedName("::");
const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent);
const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
const string matlabQualName = qualifiedName(".");
const string matlabUniqueName = qualifiedName();
const string cppName = qualifiedName("::");
const string matlabBaseName = qualifiedParent.qualifiedName(".");
const string cppBaseName = qualifiedParent.qualifiedName("::");
// emit class proxy code
// we want our class to inherit the handle class for memory purposes
@ -75,13 +73,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName,
functionNames);
wrapperFile.oss << "\n";
// Regular constructors
BOOST_FOREACH(ArgumentList a, constructor.args_list) {
for (size_t i = 0; i < constructor.nrOverloads(); i++) {
ArgumentList args = constructor.argumentList(i);
const int id = (int) functionNames.size();
constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(),
id, a);
id, args);
const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
cppName, matlabUniqueName, cppBaseName, id, a);
cppName, matlabUniqueName, cppBaseName, id, args);
wrapperFile.oss << "\n";
functionNames.push_back(wrapFunctionName);
}
@ -144,19 +144,14 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
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,
FileWriter& wrapperFile, Str wrapperName,
vector<string>& functionNames) const {
const string matlabUniqueName = qualifiedName(), cppName = qualifiedName(
"::");
const string baseCppName = wrap::qualifiedName("::", qualifiedParent);
const string matlabUniqueName = qualifiedName();
const string cppName = qualifiedName("::");
const string baseCppName = qualifiedParent.qualifiedName("::");
const int collectorInsertId = (int) functionNames.size();
const string collectorInsertFunctionName = matlabUniqueName
@ -247,128 +242,126 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
}
/* ************************************************************************* */
vector<ArgumentList> expandArgumentListsTemplate(
const vector<ArgumentList>& argLists, const string& templateArg,
const vector<string>& instName,
const std::vector<string>& expandedClassNamespace,
const string& expandedClassName) {
vector<ArgumentList> 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();
} else if (arg.type == "This") {
instArg.namespaces.assign(expandedClassNamespace.begin(),
expandedClassNamespace.end());
instArg.type = expandedClassName;
}
instArgList.push_back(instArg);
}
result.push_back(instArgList);
}
return result;
}
/* ************************************************************************* */
template<class METHOD>
map<string, METHOD> expandMethodTemplate(const map<string, METHOD>& methods,
const string& templateArg, const vector<string>& instName,
const std::vector<string>& expandedClassNamespace,
const string& expandedClassName) {
map<string, METHOD> result;
typedef pair<const string, METHOD> 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, expandedClassNamespace, expandedClassName);
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();
} else if (retVal.type1 == "This") {
instRetVal.namespaces1.assign(expandedClassNamespace.begin(),
expandedClassNamespace.end());
instRetVal.type1 = expandedClassName;
}
if (retVal.type2 == templateArg) {
instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1);
instRetVal.type2 = instName.back();
} else if (retVal.type1 == "This") {
instRetVal.namespaces2.assign(expandedClassNamespace.begin(),
expandedClassNamespace.end());
instRetVal.type2 = expandedClassName;
}
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<string>& instName,
const std::vector<string>& expandedClassNamespace,
const string& expandedClassName) {
Class inst;
inst.name = cls.name;
inst.templateArgs = cls.templateArgs;
inst.typedefName = cls.typedefName;
inst.isVirtual = cls.isVirtual;
inst.isSerializable = cls.isSerializable;
inst.qualifiedParent = cls.qualifiedParent;
inst.methods = expandMethodTemplate(cls.methods, templateArg, instName,
expandedClassNamespace, expandedClassName);
inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg,
instName, expandedClassNamespace, expandedClassName);
inst.namespaces = cls.namespaces;
inst.constructor = cls.constructor;
inst.constructor.args_list = expandArgumentListsTemplate(
cls.constructor.args_list, templateArg, instName, expandedClassNamespace,
expandedClassName);
inst.constructor.name = inst.name;
inst.deconstructor = cls.deconstructor;
Class Class::expandTemplate(const TemplateSubstitution& ts) const {
Class inst = *this;
inst.methods = expandMethodTemplate(methods, ts);
inst.static_methods = expandMethodTemplate(static_methods, ts);
inst.constructor = constructor.expandTemplate(ts);
inst.deconstructor.name = inst.name;
inst.verbose_ = cls.verbose_;
return inst;
}
/* ************************************************************************* */
vector<Class> Class::expandTemplate(const string& templateArg,
const vector<vector<string> >& instantiations) const {
vector<Class> Class::expandTemplate(Str templateArg,
const vector<Qualified>& instantiations) const {
vector<Class> result;
BOOST_FOREACH(const vector<string>& instName, instantiations) {
const string expandedName = name + instName.back();
Class inst = expandClassTemplate(*this, templateArg, instName,
this->namespaces, expandedName);
inst.name = expandedName;
BOOST_FOREACH(const Qualified& instName, instantiations) {
Qualified expandedClass = (Qualified) (*this);
expandedClass.name += instName.name;
const TemplateSubstitution ts(templateArg, instName, expandedClass);
Class inst = expandTemplate(ts);
inst.name = expandedClass.name;
inst.templateArgs.clear();
inst.typedefName = qualifiedName("::") + "<"
+ wrap::qualifiedName("::", instName) + ">";
inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::")
+ ">";
result.push_back(inst);
}
return result;
}
/* ************************************************************************* */
Class Class::expandTemplate(const string& templateArg,
const vector<string>& instantiation,
const std::vector<string>& expandedClassNamespace,
const string& expandedClassName) const {
return expandClassTemplate(*this, templateArg, instantiation,
expandedClassNamespace, expandedClassName);
void Class::addMethod(bool verbose, bool is_const, Str methodName,
const ArgumentList& argumentList, const ReturnValue& returnValue,
Str templateArgName, const vector<Qualified>& templateArgValues) {
// Check if templated
if (!templateArgName.empty() && templateArgValues.size() > 0) {
// Create method to expand
// For all values of the template argument, create a new method
BOOST_FOREACH(const Qualified& instName, templateArgValues) {
const TemplateSubstitution ts(templateArgName, instName, this->name);
// substitute template in arguments
ArgumentList expandedArgs = argumentList.expandTemplate(ts);
// do the same for return type
ReturnValue expandedRetVal = returnValue.expandTemplate(ts);
// Now stick in new overload stack with expandedMethodName key
// but note we use the same, unexpanded methodName in overload
string expandedMethodName = methodName + instName.name;
methods[expandedMethodName].addOverload(methodName, expandedArgs,
expandedRetVal, is_const, instName, verbose);
}
} else
// just add overload
methods[methodName].addOverload(methodName, argumentList, returnValue,
is_const, Qualified(), verbose);
}
/* ************************************************************************* */
std::string Class::getTypedef() const {
void Class::erase_serialization() {
Methods::iterator it = methods.find("serializable");
if (it != methods.end()) {
#ifndef WRAP_DISABLE_SERIALIZE
isSerializable = true;
#else
// cout << "Ignoring serializable() flag in class " << name << endl;
#endif
methods.erase(it);
}
it = methods.find("serialize");
if (it != methods.end()) {
#ifndef WRAP_DISABLE_SERIALIZE
isSerializable = true;
hasSerialization = true;
#else
// cout << "Ignoring serialize() flag in class " << name << endl;
#endif
methods.erase(it);
}
}
/* ************************************************************************* */
void Class::verifyAll(vector<string>& validTypes, bool& hasSerialiable) const {
hasSerialiable |= isSerializable;
// verify all of the function arguments
//TODO:verifyArguments<ArgumentList>(validTypes, constructor.args_list);
verifyArguments<StaticMethod>(validTypes, static_methods);
verifyArguments<Method>(validTypes, methods);
// verify function return types
verifyReturnTypes<StaticMethod>(validTypes, static_methods);
verifyReturnTypes<Method>(validTypes, methods);
// verify parents
if (!qualifiedParent.empty()
&& find(validTypes.begin(), validTypes.end(),
qualifiedParent.qualifiedName("::")) == validTypes.end())
throw DependencyMissing(qualifiedParent.qualifiedName("::"),
qualifiedName("::"));
}
/* ************************************************************************* */
void Class::appendInheritedMethods(const Class& cls,
const vector<Class>& classes) {
if (!cls.qualifiedParent.empty()) {
// Find parent
BOOST_FOREACH(const Class& parent, classes) {
// We found a parent class for our parent, TODO improve !
if (parent.name == cls.qualifiedParent.name) {
methods.insert(parent.methods.begin(), parent.methods.end());
appendInheritedMethods(parent, classes);
}
}
}
}
/* ************************************************************************* */
string Class::getTypedef() const {
string result;
BOOST_FOREACH(const string& namesp, namespaces) {
BOOST_FOREACH(Str namesp, namespaces) {
result += ("namespace " + namesp + " { ");
}
result += ("typedef " + typedefName + " " + name + ";");
@ -379,43 +372,22 @@ std::string Class::getTypedef() const {
}
/* ************************************************************************* */
void Class::comment_fragment(FileWriter& proxyFile) const {
proxyFile.oss << "%class " << name << ", see Doxygen page for details\n";
proxyFile.oss
<< "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
if (!constructor.args_list.empty())
proxyFile.oss << "%\n%-------Constructors-------\n";
BOOST_FOREACH(ArgumentList argList, constructor.args_list) {
proxyFile.oss << "%";
argList.emit_prototype(proxyFile, name);
proxyFile.oss << "\n";
}
constructor.comment_fragment(proxyFile);
if (!methods.empty())
proxyFile.oss << "%\n%-------Methods-------\n";
BOOST_FOREACH(const Methods::value_type& name_m, methods) {
const Method& m = name_m.second;
BOOST_FOREACH(ArgumentList argList, m.argLists) {
proxyFile.oss << "%";
argList.emit_prototype(proxyFile, m.name);
proxyFile.oss << " : returns "
<< m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl;
}
}
BOOST_FOREACH(const Methods::value_type& name_m, methods)
name_m.second.comment_fragment(proxyFile);
if (!static_methods.empty())
proxyFile.oss << "%\n%-------Static Methods-------\n";
BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
const StaticMethod& m = name_m.second;
BOOST_FOREACH(ArgumentList argList, m.argLists) {
proxyFile.oss << "%";
argList.emit_prototype(proxyFile, m.name);
proxyFile.oss << " : returns "
<< m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl;
}
}
BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods)
name_m.second.comment_fragment(proxyFile);
if (hasSerialization) {
proxyFile.oss << "%\n%-------Serialization Interface-------\n";
@ -429,23 +401,24 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
/* ************************************************************************* */
void Class::serialization_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const std::string& wrapperName,
std::vector<std::string>& functionNames) const {
FileWriter& wrapperFile, Str wrapperName,
vector<string>& functionNames) const {
//void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
//{
// typedef boost::shared_ptr<Point3> Shared;
// checkArguments("string_serialize",nargout,nargin-1,0);
// Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
// std::ostringstream out_archive_stream;
// ostringstream out_archive_stream;
// boost::archive::text_oarchive out_archive(out_archive_stream);
// out_archive << *obj;
// out[0] = wrap< string >(out_archive_stream.str());
//}
int serialize_id = functionNames.size();
const string matlabQualName = qualifiedName("."), matlabUniqueName =
qualifiedName(), cppClassName = qualifiedName("::");
const string matlabQualName = qualifiedName(".");
const string matlabUniqueName = qualifiedName();
const string cppClassName = qualifiedName("::");
const string wrapFunctionNameSerialize = matlabUniqueName
+ "_string_serialize_" + boost::lexical_cast<string>(serialize_id);
functionNames.push_back(wrapFunctionNameSerialize);
@ -469,7 +442,7 @@ void Class::serialization_fragments(FileWriter& proxyFile,
<< ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
// Serialization boilerplate
wrapperFile.oss << " std::ostringstream out_archive_stream;\n";
wrapperFile.oss << " ostringstream out_archive_stream;\n";
wrapperFile.oss
<< " boost::archive::text_oarchive out_archive(out_archive_stream);\n";
wrapperFile.oss << " out_archive << *obj;\n";
@ -520,22 +493,23 @@ void Class::serialization_fragments(FileWriter& proxyFile,
/* ************************************************************************* */
void Class::deserialization_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const std::string& wrapperName,
std::vector<std::string>& functionNames) const {
FileWriter& wrapperFile, Str wrapperName,
vector<string>& functionNames) const {
//void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
//{
// typedef boost::shared_ptr<Point3> Shared;
// checkArguments("Point3.string_deserialize",nargout,nargin,1);
// string serialized = unwrap< string >(in[0]);
// std::istringstream in_archive_stream(serialized);
// istringstream in_archive_stream(serialized);
// boost::archive::text_iarchive in_archive(in_archive_stream);
// Shared output(new Point3());
// in_archive >> *output;
// out[0] = wrap_shared_ptr(output,"Point3", false);
//}
int deserialize_id = functionNames.size();
const string matlabQualName = qualifiedName("."), matlabUniqueName =
qualifiedName(), cppClassName = qualifiedName("::");
const string matlabQualName = qualifiedName(".");
const string matlabUniqueName = qualifiedName();
const string cppClassName = qualifiedName("::");
const string wrapFunctionNameDeserialize = matlabUniqueName
+ "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id);
functionNames.push_back(wrapFunctionNameDeserialize);
@ -553,7 +527,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
// string argument with deserialization boilerplate
wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n";
wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n";
wrapperFile.oss << " istringstream in_archive_stream(serialized);\n";
wrapperFile.oss
<< " boost::archive::text_iarchive in_archive(in_archive_stream);\n";
wrapperFile.oss << " Shared output(new " << cppClassName << "());\n";
@ -604,9 +578,21 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
}
/* ************************************************************************* */
std::string Class::getSerializationExport() const {
string Class::getSerializationExport() const {
//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal");
return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \""
+ qualifiedName() + "\");";
}
/* ************************************************************************* */
void Class::python_wrapper(FileWriter& wrapperFile) const {
wrapperFile.oss << "class_<" << name << ">(\"" << name << "\")\n";
constructor.python_wrapper(wrapperFile, name);
BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values)
m.python_wrapper(wrapperFile, name);
BOOST_FOREACH(const Method& m, methods | boost::adaptors::map_values)
m.python_wrapper(wrapperFile, name);
wrapperFile.oss << ";\n\n";
}
/* ************************************************************************* */

118
wrap/Class.h
View File

@ -19,67 +19,119 @@
#pragma once
#include <string>
#include <map>
#include "Constructor.h"
#include "Deconstructor.h"
#include "Method.h"
#include "StaticMethod.h"
#include "TypeAttributesTable.h"
#include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp>
#include <string>
#include <map>
namespace wrap {
/// Class has name, constructors, methods
struct Class {
class Class: public Qualified {
typedef std::map<std::string, Method> Methods;
Methods methods; ///< Class methods
public:
typedef const std::string& Str;
typedef std::map<std::string, StaticMethod> StaticMethods;
/// Constructor creates an empty class
Class(bool verbose=true) : isVirtual(false), isSerializable(false), hasSerialization(false), verbose_(verbose) {}
// Then the instance variables are set directly by the Module constructor
std::string name; ///< Class name
std::vector<std::string> templateArgs; ///< Template arguments
std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name]
bool isVirtual; ///< Whether the class is part of a virtual inheritance chain
bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports
bool hasSerialization; ///< Whether we should create the serialization functions
std::vector<std::string> qualifiedParent; ///< The *single* parent - the last string is the parent class name, preceededing elements are a namespace stack
Methods methods; ///< Class methods
StaticMethods static_methods; ///< Static methods
std::vector<std::string> namespaces; ///< Stack of namespaces
Constructor constructor; ///< Class constructors
Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object
bool verbose_; ///< verbose flag
std::vector<std::string> templateArgs; ///< Template arguments
std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name]
bool isVirtual; ///< Whether the class is part of a virtual inheritance chain
bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports
bool hasSerialization; ///< Whether we should create the serialization functions
Qualified qualifiedParent; ///< The *single* parent
StaticMethods static_methods; ///< Static methods
Constructor constructor; ///< Class constructors
Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object
bool verbose_; ///< verbose flag
/// Constructor creates an empty class
Class(bool verbose = true) :
isVirtual(false), isSerializable(false), hasSerialization(false), deconstructor(
verbose), verbose_(verbose) {
}
size_t nrMethods() const {
return methods.size();
}
Method& method(Str name) {
return methods.at(name);
}
bool exists(Str name) const {
return methods.find(name) != methods.end();
}
// And finally MATLAB code is emitted, methods below called by Module::matlab_code
void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& wrapperFile, std::vector<std::string>& functionNames) const; ///< emit proxy class
void matlab_proxy(Str toolboxPath, Str wrapperName,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
std::vector<std::string>& functionNames) const; ///< emit proxy class
std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter
Class expandTemplate(const TemplateSubstitution& ts) const;
std::vector<Class> expandTemplate(const std::string& templateArg, const std::vector<std::vector<std::string> >& instantiations) const;
std::vector<Class> expandTemplate(Str templateArg,
const std::vector<Qualified>& instantiations) const;
Class expandTemplate(const std::string& templateArg, const std::vector<std::string>& instantiation, const std::vector<std::string>& expandedClassNamespace, const std::string& expandedClassName) const;
/// Add potentially overloaded, potentially templated method
void addMethod(bool verbose, bool is_const, Str methodName,
const ArgumentList& argumentList, const ReturnValue& returnValue,
Str templateArgName, const std::vector<Qualified>& templateArgValues);
// The typedef line for this class, if this class is a typedef, otherwise returns an empty string.
/// Post-process classes for serialization markers
void erase_serialization(); // non-const !
/// verify all of the function arguments
void verifyAll(std::vector<std::string>& functionNames,
bool& hasSerialiable) const;
void appendInheritedMethods(const Class& cls,
const std::vector<Class>& classes);
/// The typedef line for this class, if this class is a typedef, otherwise returns an empty string.
std::string getTypedef() const;
// Returns the string for an export flag
/// Returns the string for an export flag
std::string getSerializationExport() const;
// Creates a member function that performs serialization
/// Creates a member function that performs serialization
void serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& wrapperName, std::vector<std::string>& functionNames) const;
Str wrapperName, std::vector<std::string>& functionNames) const;
// Creates a static member function that performs deserialization
/// Creates a static member function that performs deserialization
void deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& wrapperName, std::vector<std::string>& functionNames) const;
Str wrapperName, std::vector<std::string>& functionNames) const;
// emit python wrapper
void python_wrapper(FileWriter& wrapperFile) const;
friend std::ostream& operator<<(std::ostream& os, const Class& cls) {
os << "class " << cls.name << "{\n";
os << cls.constructor << ";\n";
BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values)
os << m << ";\n";
BOOST_FOREACH(const Method& m, cls.methods | boost::adaptors::map_values)
os << m << ";\n";
os << "};" << std::endl;
return os;
}
private:
void pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const std::string& wrapperName, std::vector<std::string>& functionNames) const;
void comment_fragment(FileWriter& proxyFile) const;
void pointer_constructor_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, Str wrapperName,
std::vector<std::string>& functionNames) const;
void comment_fragment(FileWriter& proxyFile) const;
};
} // \namespace wrap

74
wrap/Constructor.cpp
View File

@ -29,52 +29,55 @@
using namespace std;
using namespace wrap;
/* ************************************************************************* */
string Constructor::matlab_wrapper_name(const string& className) const {
string Constructor::matlab_wrapper_name(Str className) const {
string str = "new_" + className;
return str;
}
/* ************************************************************************* */
void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperName,
bool hasParent, const int id, const ArgumentList args) const {
void Constructor::proxy_fragment(FileWriter& file,
const std::string& wrapperName, bool hasParent, const int id,
const ArgumentList args) const {
size_t nrArgs = args.size();
// check for number of arguments...
file.oss << " elseif nargin == " << nrArgs;
if (nrArgs>0) file.oss << " && ";
if (nrArgs > 0)
file.oss << " && ";
// ...and their types
bool first = true;
for(size_t i=0;i<nrArgs;i++) {
if (!first) file.oss << " && ";
file.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')";
first=false;
for (size_t i = 0; i < nrArgs; i++) {
if (!first)
file.oss << " && ";
file.oss << "isa(varargin{" << i + 1 << "},'" << args[i].matlabClass(".")
<< "')";
first = false;
}
// emit code for calling constructor
if(hasParent)
if (hasParent)
file.oss << "\n [ my_ptr, base_ptr ] = ";
else
file.oss << "\n my_ptr = ";
file.oss << wrapperName << "(" << id;
// emit constructor arguments
for(size_t i=0;i<nrArgs;i++) {
for (size_t i = 0; i < nrArgs; i++) {
file.oss << ", ";
file.oss << "varargin{" << i+1 << "}";
file.oss << "varargin{" << i + 1 << "}";
}
file.oss << ");\n";
}
/* ************************************************************************* */
string Constructor::wrapper_fragment(FileWriter& file,
const string& cppClassName,
const string& matlabUniqueName,
const string& cppBaseClassName,
int id,
const ArgumentList& al) const {
string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName,
Str matlabUniqueName, Str cppBaseClassName, int id,
const ArgumentList& al) const {
const string wrapFunctionName = matlabUniqueName + "_constructor_" + boost::lexical_cast<string>(id);
const string wrapFunctionName = matlabUniqueName + "_constructor_"
+ boost::lexical_cast<string>(id);
file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
file.oss << "void " << wrapFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])"
<< endl;
file.oss << "{\n";
file.oss << " mexAtExit(&_deleteAllObjects);\n";
//Typedef boost::shared_ptr
@ -82,22 +85,29 @@ string Constructor::wrapper_fragment(FileWriter& file,
file.oss << "\n";
//Check to see if there will be any arguments and remove {} for consiseness
if(al.size() > 0)
if (al.size() > 0)
al.matlab_unwrap(file); // unwrap arguments
file.oss << " Shared *self = new Shared(new " << cppClassName << "(" << al.names() << "));" << endl;
file.oss << " Shared *self = new Shared(new " << cppClassName << "("
<< al.names() << "));" << endl;
file.oss << " collector_" << matlabUniqueName << ".insert(self);\n";
if(verbose_)
if (verbose_)
file.oss << " std::cout << \"constructed \" << self << std::endl;" << endl;
file.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl;
file.oss << " *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;" << endl;
file.oss
<< " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);"
<< endl;
file.oss << " *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;"
<< endl;
// 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(!cppBaseClassName.empty()) {
if (!cppBaseClassName.empty()) {
file.oss << "\n";
file.oss << " typedef boost::shared_ptr<" << cppBaseClassName << "> SharedBase;\n";
file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
file.oss << " *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);\n";
file.oss << " typedef boost::shared_ptr<" << cppBaseClassName
<< "> SharedBase;\n";
file.oss
<< " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
file.oss
<< " *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);\n";
}
file.oss << "}" << endl;
@ -106,3 +116,9 @@ string Constructor::wrapper_fragment(FileWriter& file,
}
/* ************************************************************************* */
void Constructor::python_wrapper(FileWriter& wrapperFile, Str className) const {
wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_
<< ");\n";
}
/* ************************************************************************* */

52
wrap/Constructor.h
View File

@ -18,7 +18,7 @@
#pragma once
#include "Argument.h"
#include "OverloadedFunction.h"
#include <string>
#include <vector>
@ -26,42 +26,64 @@
namespace wrap {
// Constructor class
struct Constructor {
struct Constructor: public OverloadedFunction {
typedef const std::string& Str;
/// Constructor creates an empty class
Constructor(bool verbose = false) :
verbose_(verbose) {
Constructor(bool verbose = false) {
verbose_ = verbose;
}
// Then the instance variables are set directly by the Module constructor
std::vector<ArgumentList> args_list;
std::string name;
bool verbose_;
Constructor expandTemplate(const TemplateSubstitution& ts) const {
Constructor inst = *this;
inst.argLists_ = expandArgumentListsTemplate(ts);
inst.name_ = ts.expandedClassName();
return inst;
}
// MATLAB code generation
// toolboxPath is main toolbox directory, e.g., ../matlab
// classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m
/// wrapper name
std::string matlab_wrapper_name(const std::string& className) const;
std::string matlab_wrapper_name(Str className) const;
void comment_fragment(FileWriter& proxyFile) const {
if (nrOverloads() > 0)
proxyFile.oss << "%\n%-------Constructors-------\n";
for (size_t i = 0; i < nrOverloads(); i++) {
proxyFile.oss << "%";
argumentList(i).emit_prototype(proxyFile, name_);
proxyFile.oss << "\n";
}
}
/**
* Create fragment to select constructor in proxy class, e.g.,
* if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end
*/
void proxy_fragment(FileWriter& file, const std::string& wrapperName,
bool hasParent, const int id, const ArgumentList args) const;
void proxy_fragment(FileWriter& file, Str wrapperName, bool hasParent,
const int id, const ArgumentList args) const;
/// cpp wrapper
std::string wrapper_fragment(FileWriter& file,
const std::string& cppClassName, const std::string& matlabUniqueName,
const std::string& cppBaseClassName, int id,
std::string wrapper_fragment(FileWriter& file, Str cppClassName,
Str matlabUniqueName, Str cppBaseClassName, int id,
const ArgumentList& al) const;
/// constructor function
void generate_construct(FileWriter& file, const std::string& cppClassName,
void generate_construct(FileWriter& file, Str cppClassName,
std::vector<ArgumentList>& args_list) const;
// emit python wrapper
void python_wrapper(FileWriter& wrapperFile, Str className) const;
friend std::ostream& operator<<(std::ostream& os, const Constructor& m) {
for (size_t i = 0; i < m.nrOverloads(); i++)
os << m.name_ << m.argLists_[i];
return os;
}
};
} // \namespace wrap

27
wrap/FileWriter.cpp
View File

@ -15,14 +15,15 @@ using namespace std;
using namespace wrap;
/* ************************************************************************* */
FileWriter::FileWriter(const string& filename, bool verbose, const string& comment_str)
: verbose_(verbose),filename_(filename), comment_str_(comment_str)
{
FileWriter::FileWriter(const string& filename, bool verbose,
const string& comment_str) :
verbose_(verbose), filename_(filename), comment_str_(comment_str) {
}
/* ************************************************************************* */
void FileWriter::emit(bool add_header, bool force_overwrite) const {
if (verbose_) cerr << "generating " << filename_ << " ";
if (verbose_)
cerr << "generating " << filename_ << " ";
// read in file if it exists
string existing_contents;
bool file_exists = true;
@ -35,23 +36,17 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const {
// Only write a file if it is new, an update, or overwrite is forced
string new_contents = oss.str();
if (force_overwrite || !file_exists || existing_contents != new_contents) {
ofstream ofs(filename_.c_str(), ios::binary); // Binary to use LF line endings instead of CRLF
if (!ofs) throw CantOpenFile(filename_);
// Binary to use LF line endings instead of CRLF
ofstream ofs(filename_.c_str(), ios::binary);
if (!ofs)
throw CantOpenFile(filename_);
// dump in stringstream
ofs << new_contents;
ofs.close();
if (verbose_) cerr << " ...complete" << endl;
// Add small message whenever writing a new file and not running in full verbose mode
if (!verbose_)
cout << "wrap: generating " << filename_ << endl;
} else {
if (verbose_) cerr << " ...no update" << endl;
}
if (verbose_)
cerr << " ...no update" << endl;
}
/* ************************************************************************* */

133
wrap/FullyOverloadedFunction.h Normal file
View File

@ -0,0 +1,133 @@
/* ----------------------------------------------------------------------------
* 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 FullyOverloadedFunction.h
* @brief Function that can be fully overloaded: arguments and return values
* @author Frank Dellaert
* @date Nov 13, 2014
**/
#pragma once
#include "OverloadedFunction.h"
namespace wrap {
/**
* Signature Overload (including return value)
*/
class SignatureOverloads: public ArgumentOverloads {
protected:
std::vector<ReturnValue> returnVals_;
public:
const ReturnValue& returnValue(size_t i) const {
return returnVals_.at(i);
}
void push_back(const ArgumentList& args, const ReturnValue& retVal) {
argLists_.push_back(args);
returnVals_.push_back(retVal);
}
void verifyReturnTypes(const std::vector<std::string>& validtypes,
const std::string& s) const {
BOOST_FOREACH(const ReturnValue& retval, returnVals_) {
retval.type1.verify(validtypes, s);
if (retval.isPair)
retval.type2.verify(validtypes, s);
}
}
// TODO use transform ?
std::vector<ReturnValue> expandReturnValuesTemplate(
const TemplateSubstitution& ts) const {
std::vector<ReturnValue> result;
BOOST_FOREACH(const ReturnValue& retVal, returnVals_) {
ReturnValue instRetVal = retVal.expandTemplate(ts);
result.push_back(instRetVal);
}
return result;
}
/// Expand templates, imperative !
void expandTemplate(const TemplateSubstitution& ts) {
// substitute template in arguments
argLists_ = expandArgumentListsTemplate(ts);
// do the same for return types
returnVals_ = expandReturnValuesTemplate(ts);
}
// emit a list of comments, one for each overload
void usage_fragment(FileWriter& proxyFile, const std::string& name) const {
unsigned int argLCount = 0;
BOOST_FOREACH(ArgumentList argList, argLists_) {
argList.emit_prototype(proxyFile, name);
if (argLCount != nrOverloads() - 1)
proxyFile.oss << ", ";
else
proxyFile.oss << " : returns " << returnValue(0).return_type(false)
<< std::endl;
argLCount++;
}
}
// emit a list of comments, one for each overload
void comment_fragment(FileWriter& proxyFile, const std::string& name) const {
size_t i = 0;
BOOST_FOREACH(ArgumentList argList, argLists_) {
proxyFile.oss << "%";
argList.emit_prototype(proxyFile, name);
proxyFile.oss << " : returns " << returnVals_[i++].return_type(false)
<< std::endl;
}
}
friend std::ostream& operator<<(std::ostream& os,
const SignatureOverloads& overloads) {
for (size_t i = 0; i < overloads.nrOverloads(); i++)
os << overloads.returnVals_[i] << overloads.argLists_[i] << std::endl;
return os;
}
};
class FullyOverloadedFunction: public Function, public SignatureOverloads {
public:
bool addOverload(const std::string& name, const ArgumentList& args,
const ReturnValue& retVal, const Qualified& instName = Qualified(),
bool verbose = false) {
bool first = initializeOrCheck(name, instName, verbose);
SignatureOverloads::push_back(args, retVal);
return first;
}
};
// Templated checking functions
// TODO: do this via polymorphism, use transform ?
template<class F>
inline void verifyReturnTypes(const std::vector<std::string>& validTypes,
const std::map<std::string, F>& vt) {
typedef typename std::map<std::string, F>::value_type NamedMethod;
BOOST_FOREACH(const NamedMethod& namedMethod, vt)
namedMethod.second.verifyReturnTypes(validTypes);
}
} // \namespace wrap

57
wrap/Function.cpp Normal file
View File

@ -0,0 +1,57 @@
/* ----------------------------------------------------------------------------
* 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 Function.ccp
* @author Frank Dellaert
* @date Nov 13, 2014
**/
#include "Function.h"
#include "utilities.h"
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <fstream>
using namespace std;
using namespace wrap;
/* ************************************************************************* */
bool Function::initializeOrCheck(const std::string& name,
const Qualified& instName, bool verbose) {
if (name.empty())
throw std::runtime_error(
"Function::initializeOrCheck called with empty name");
// Check if this overload is give to the correct method
if (name_.empty()) {
name_ = name;
templateArgValue_ = instName;
verbose_ = verbose;
return true;
} else {
if (name_ != name || templateArgValue_ != instName || verbose_ != verbose)
throw std::runtime_error(
"Function::initializeOrCheck called with different arguments: with name "
+ name + " instead of expected " + name_
+ ", or with template argument " + instName.qualifiedName(":")
+ " instead of expected " + templateArgValue_.qualifiedName(":"));
return false;
}
}
/* ************************************************************************* */

56
wrap/Function.h Normal file
View File

@ -0,0 +1,56 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Function.h
* @brief Base class for global functions and methods
* @author Frank Dellaert
* @date Nov 13, 2014
**/
#pragma once
#include "Argument.h"
namespace wrap {
/// Function class
class Function {
protected:
std::string name_; ///< name of method
Qualified templateArgValue_; ///< value of template argument if applicable
bool verbose_;
public:
/**
* @brief first time, fill in instance variables, otherwise check if same
* @return true if first time, false thereafter
*/
bool initializeOrCheck(const std::string& name, const Qualified& instName =
Qualified(), bool verbose = false);
std::string name() const {
return name_;
}
std::string matlabName() const {
if (templateArgValue_.empty())
return name_;
else
return name_ + templateArgValue_.name;
}
};
} // \namespace wrap

77
wrap/GlobalFunction.cpp
View File

@ -16,38 +16,31 @@ namespace wrap {
using namespace std;
/* ************************************************************************* */
void GlobalFunction::addOverload(bool verbose, const std::string& name,
void GlobalFunction::addOverload(const Qualified& overload,
const ArgumentList& args, const ReturnValue& retVal,
const StrVec& ns_stack) {
this->verbose_ = verbose;
this->name = name;
this->argLists.push_back(args);
this->returnVals.push_back(retVal);
this->namespaces.push_back(ns_stack);
const Qualified& instName, bool verbose) {
string name(overload.name);
FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose);
overloads.push_back(overload);
}
/* ************************************************************************* */
void GlobalFunction::matlab_proxy(const std::string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const {
void GlobalFunction::matlab_proxy(const string& toolboxPath,
const string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, vector<string>& functionNames) const {
// cluster overloads with same namespace
// create new GlobalFunction structures around namespaces - same namespaces and names are overloads
// map of namespace to global function
typedef map<string, GlobalFunction> GlobalFunctionMap;
GlobalFunctionMap grouped_functions;
for (size_t i = 0; i < namespaces.size(); ++i) {
StrVec ns = namespaces.at(i);
string str_ns = qualifiedName("", ns, "");
ReturnValue ret = returnVals.at(i);
ArgumentList args = argLists.at(i);
if (!grouped_functions.count(str_ns))
grouped_functions[str_ns] = GlobalFunction(name, verbose_);
grouped_functions[str_ns].argLists.push_back(args);
grouped_functions[str_ns].returnVals.push_back(ret);
grouped_functions[str_ns].namespaces.push_back(ns);
for (size_t i = 0; i < overloads.size(); ++i) {
Qualified overload = overloads.at(i);
// use concatenated namespaces as key
string str_ns = qualifiedName("", overload.namespaces);
const ReturnValue& ret = returnValue(i);
const ArgumentList& args = argumentList(i);
grouped_functions[str_ns].addOverload(overload, args, ret);
}
size_t lastcheck = grouped_functions.size();
@ -60,37 +53,34 @@ void GlobalFunction::matlab_proxy(const std::string& toolboxPath,
}
/* ************************************************************************* */
void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const {
void GlobalFunction::generateSingleFunction(const string& toolboxPath,
const string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, vector<string>& functionNames) const {
// create the folder for the namespace
const StrVec& ns = namespaces.front();
createNamespaceStructure(ns, toolboxPath);
const Qualified& overload1 = overloads.front();
createNamespaceStructure(overload1.namespaces, toolboxPath);
// open destination mfunctionFileName
string mfunctionFileName = toolboxPath;
if (!ns.empty())
mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns);
mfunctionFileName += "/" + name + ".m";
string mfunctionFileName = overload1.matlabName(toolboxPath);
FileWriter mfunctionFile(mfunctionFileName, verbose_, "%");
// get the name of actual matlab object
const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName =
qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name);
const string matlabQualName = overload1.qualifiedName(".");
const string matlabUniqueName = overload1.qualifiedName("");
const string cppName = overload1.qualifiedName("::");
mfunctionFile.oss << "function varargout = " << name << "(varargin)\n";
mfunctionFile.oss << "function varargout = " << name_ << "(varargin)\n";
for (size_t overload = 0; overload < argLists.size(); ++overload) {
const ArgumentList& args = argLists[overload];
const ReturnValue& returnVal = returnVals[overload];
for (size_t i = 0; i < nrOverloads(); ++i) {
const ArgumentList& args = argumentList(i);
const ReturnValue& returnVal = returnValue(i);
const int id = functionNames.size();
// Output proxy matlab code
mfunctionFile.oss << " " << (overload == 0 ? "" : "else");
argLists[overload].emit_conditional_call(mfunctionFile,
returnVals[overload], wrapperName, id, true); // true omits "this"
mfunctionFile.oss << " " << (i == 0 ? "" : "else");
args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this"
// Output C++ wrapper code
@ -114,7 +104,7 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
// call method with default type and wrap result
if (returnVal.type1 != "void")
if (returnVal.type1.name != "void")
returnVal.wrap_result(cppName + "(" + args.names() + ")", file,
typeAttributes);
else
@ -137,6 +127,11 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
mfunctionFile.emit(true);
}
/* ************************************************************************* */
void GlobalFunction::python_wrapper(FileWriter& wrapperFile) const {
wrapperFile.oss << "def(\"" << name_ << "\", " << name_ << ");\n";
}
/* ************************************************************************* */
} // \namespace wrap

36
wrap/GlobalFunction.h
View File

@ -9,43 +9,35 @@
#pragma once
#include "Argument.h"
#include "ReturnValue.h"
#include "FullyOverloadedFunction.h"
namespace wrap {
struct GlobalFunction {
struct GlobalFunction: public FullyOverloadedFunction {
typedef std::vector<std::string> StrVec;
std::vector<Qualified> overloads; ///< Stack of qualified names
bool verbose_;
std::string name;
// adds an overloaded version of this function,
void addOverload(const Qualified& overload, const ArgumentList& args,
const ReturnValue& retVal, const Qualified& instName = Qualified(),
bool verbose = false);
// each overload, regardless of namespace
std::vector<ArgumentList> argLists; ///< arugments for each overload
std::vector<ReturnValue> returnVals; ///< returnVals for each overload
std::vector<StrVec> namespaces; ///< Stack of namespaces
// Constructor only used in Module
GlobalFunction(bool verbose = true) :
verbose_(verbose) {
void verifyArguments(const std::vector<std::string>& validArgs) const {
SignatureOverloads::verifyArguments(validArgs, name_);
}
// Used to reconstruct
GlobalFunction(const std::string& name_, bool verbose = true) :
verbose_(verbose), name(name_) {
void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
SignatureOverloads::verifyReturnTypes(validtypes, name_);
}
// adds an overloaded version of this function
void addOverload(bool verbose, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal,
const StrVec& ns_stack);
// codegen function called from Module to build the cpp and matlab versions of the function
void matlab_proxy(const std::string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const;
// emit python wrapper
void python_wrapper(FileWriter& wrapperFile) const;
private:
// Creates a single global function - all in same namespace

162
wrap/Method.cpp
View File

@ -29,155 +29,53 @@ using namespace std;
using namespace wrap;
/* ************************************************************************* */
void Method::addOverload(bool verbose, bool is_const, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal) {
this->verbose_ = verbose;
this->is_const_ = is_const;
this->name = name;
this->argLists.push_back(args);
this->returnVals.push_back(retVal);
bool Method::addOverload(Str name, const ArgumentList& args,
const ReturnValue& retVal, bool is_const, const Qualified& instName,
bool verbose) {
bool first = StaticMethod::addOverload(name, args, retVal, instName, verbose);
if (first)
is_const_ = is_const;
else if (is_const && !is_const_)
throw std::runtime_error(
"Method::addOverload now designated as const whereas before it was not");
else if (!is_const && is_const_)
throw std::runtime_error(
"Method::addOverload now designated as non-const whereas before it was");
return first;
}
/* ************************************************************************* */
void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile,
const string& cppClassName, const std::string& matlabQualName,
const std::string& matlabUniqueName, const string& wrapperName,
const TypeAttributesTable& typeAttributes,
vector<string>& functionNames) const {
// Create function header
file.oss << " function varargout = " << name << "(this, varargin)\n";
// Emit comments for documentation
string up_name = boost::to_upper_copy(name);
file.oss << " % " << up_name << " usage: ";
unsigned int argLCount = 0;
BOOST_FOREACH(ArgumentList argList, argLists) {
argList.emit_prototype(file, name);
if (argLCount != argLists.size() - 1)
file.oss << ", ";
else
file.oss << " : returns "
<< returnVals[0].return_type(false, returnVals[0].pair) << endl;
argLCount++;
}
// Emit URL to Doxygen page
file.oss << " % "
<< "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
<< endl;
// Document all overloads, if any
if (argLists.size() > 1) {
file.oss << " % " << "" << endl;
file.oss << " % " << "Method Overloads" << endl;
BOOST_FOREACH(ArgumentList argList, argLists) {
file.oss << " % ";
argList.emit_prototype(file, name);
file.oss << endl;
}
}
// Handle special case of single overload with all numeric arguments
if (argLists.size() == 1 && argLists[0].allScalar()) {
// Output proxy matlab code
file.oss << " ";
const int id = (int) functionNames.size();
argLists[0].emit_call(file, returnVals[0], wrapperName, id);
// Output C++ wrapper code
const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
matlabUniqueName, 0, id, typeAttributes);
// Add to function list
functionNames.push_back(wrapFunctionName);
} else {
// Check arguments for all overloads
for (size_t overload = 0; overload < argLists.size(); ++overload) {
// Output proxy matlab code
file.oss << " " << (overload == 0 ? "" : "else");
const int id = (int) functionNames.size();
argLists[overload].emit_conditional_call(file, returnVals[overload],
wrapperName, id);
// Output C++ wrapper code
const string wrapFunctionName = wrapper_fragment(wrapperFile,
cppClassName, matlabUniqueName, overload, id, typeAttributes);
// Add to function list
functionNames.push_back(wrapFunctionName);
}
file.oss << " else\n";
file.oss
<< " error('Arguments do not match any overload of function "
<< matlabQualName << "." << name << "');" << endl;
file.oss << " end\n";
}
file.oss << " end\n";
void Method::proxy_header(FileWriter& proxyFile) const {
proxyFile.oss << " function varargout = " << matlabName()
<< "(this, varargin)\n";
}
/* ************************************************************************* */
string Method::wrapper_fragment(FileWriter& file, const string& cppClassName,
const string& matlabUniqueName, int overload, int id,
const TypeAttributesTable& typeAttributes) const {
// generate code
const string wrapFunctionName = matlabUniqueName + "_" + name + "_"
+ boost::lexical_cast<string>(id);
const ArgumentList& args = argLists[overload];
const ReturnValue& returnVal = returnVals[overload];
// call
file.oss << "void " << wrapFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
// start
file.oss << "{\n";
if (returnVal.isPair) {
if (returnVal.category1 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<"
<< returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1
<< ";" << endl;
if (returnVal.category2 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<"
<< returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2
<< ";" << endl;
} else if (returnVal.category1 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::")
<< "> Shared" << returnVal.type1 << ";" << endl;
file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;"
<< endl;
string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
Str matlabUniqueName, const ArgumentList& args,
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
const Qualified& instName) const {
// check arguments
// extra argument obj -> nargin-1 is passed !
// example: checkArguments("equals",nargout,nargin-1,2);
file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1,"
wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1,"
<< args.size() << ");\n";
// get class pointer
// example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName
wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName
<< ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
// unwrap arguments, see Argument.cpp
args.matlab_unwrap(file, 1);
// unwrap arguments, see Argument.cpp, we start at 1 as first is obj
args.matlab_unwrap(wrapperFile, 1);
// call method and wrap result
// example: out[0]=wrap<bool>(self->return_field(t));
if (returnVal.type1 != "void")
returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file,
typeAttributes);
else
file.oss << " obj->" + name + "(" + args.names() + ");\n";
// example: out[0]=wrap<bool>(obj->return_field(t));
string expanded = "obj->" + name_;
if (!instName.empty())
expanded += ("<" + instName.qualifiedName("::") + ">");
// finish
file.oss << "}\n";
return wrapFunctionName;
return expanded;
}
/* ************************************************************************* */

63
wrap/Method.h
View File

@ -18,49 +18,46 @@
#pragma once
#include "Argument.h"
#include "ReturnValue.h"
#include "TypeAttributesTable.h"
#include <string>
#include <list>
#include "StaticMethod.h"
namespace wrap {
/// Method class
struct Method {
class Method: public StaticMethod {
/// Constructor creates empty object
Method(bool verbose = true) :
verbose_(verbose), is_const_(false) {}
// Then the instance variables are set directly by the Module constructor
bool verbose_;
bool is_const_;
std::string name;
std::vector<ArgumentList> argLists;
std::vector<ReturnValue> returnVals;
// The first time this function is called, it initializes the class members
// with those in rhs, but in subsequent calls it adds additional argument
// lists as function overloads.
void addOverload(bool verbose, bool is_const, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal);
public:
// MATLAB code generation
// classPath is class directory, e.g., ../matlab/@Point2
void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
std::vector<std::string>& functionNames) const;
typedef const std::string& Str;
bool addOverload(Str name, const ArgumentList& args,
const ReturnValue& retVal, bool is_const, const Qualified& instName =
Qualified(), bool verbose = false);
virtual bool isStatic() const {
return false;
}
virtual bool isConst() const {
return is_const_;
}
friend std::ostream& operator<<(std::ostream& os, const Method& m) {
for (size_t i = 0; i < m.nrOverloads(); i++)
os << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
return os;
}
private:
std::string wrapper_fragment(FileWriter& file,
const std::string& cppClassName,
const std::string& matlabUniqueName,
int overload,
int id,
const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
// Emit method header
void proxy_header(FileWriter& proxyFile) const;
virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
Str matlabUniqueName, const ArgumentList& args,
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
const Qualified& instName) const;
};
} // \namespace wrap

457
wrap/Module.cpp
View File

@ -62,17 +62,23 @@ typedef rule<BOOST_SPIRIT_CLASSIC_NS::phrase_scanner_t> Rule;
/* ************************************************************************* */
/* ************************************************************************* */
void handle_possible_template(vector<Class>& classes, const Class& cls, const string& templateArgument, const vector<vector<string> >& instantiations) {
if(instantiations.empty()) {
classes.push_back(cls);
} else {
vector<Class> classInstantiations = cls.expandTemplate(templateArgument, instantiations);
BOOST_FOREACH(const Class& c, classInstantiations) {
classes.push_back(c);
}
}
}
// If a number of template arguments were given, generate a number of expanded
// class names, e.g., PriorFactor -> PriorFactorPose2, and add those classes
static void handle_possible_template(vector<Class>& classes, const Class& cls,
const vector<Qualified>& instantiations) {
if (cls.templateArgs.empty() || instantiations.empty()) {
classes.push_back(cls);
} else {
if (cls.templateArgs.size() != 1)
throw std::runtime_error(
"In-line template instantiations only handle a single template argument");
vector<Class> classInstantiations = //
cls.expandTemplate(cls.templateArgs.front(), instantiations);
BOOST_FOREACH(const Class& c, classInstantiations)
classes.push_back(c);
}
}
/* ************************************************************************* */
Module::Module(const std::string& moduleName, bool enable_verbose)
: name(moduleName), verbose(enable_verbose)
@ -94,28 +100,9 @@ Module::Module(const string& interfacePath,
/* ************************************************************************* */
void Module::parseMarkup(const std::string& data) {
// these variables will be imperatively updated to gradually build [cls]
// The parse imperatively :-( updates variables gradually during parse
// The one with postfix 0 are used to reset the variables after parse.
string methodName, methodName0;
bool isConst, isConst0 = false;
ReturnValue retVal0, retVal;
Argument arg0, arg;
ArgumentList args0, args;
vector<string> arg_dup; ///keep track of duplicates
Constructor constructor0(verbose), constructor(verbose);
Deconstructor deconstructor0(verbose), deconstructor(verbose);
StaticMethod static_method0(verbose), static_method(verbose);
Class cls0(verbose),cls(verbose);
GlobalFunction globalFunc0(verbose), globalFunc(verbose);
ForwardDeclaration fwDec0, fwDec;
vector<string> namespaces, /// current namespace tag
namespaces_return; /// namespace for current return type
string templateArgument;
vector<string> templateInstantiationNamespace;
vector<vector<string> > templateInstantiations;
TemplateInstantiationTypedef singleInstantiation, singleInstantiation0;
string include_path = "";
const string null_str = "";
vector<string> namespaces; // current namespace tag
//----------------------------------------------------------------------------
// Grammar with actions that build the Class object. Actions are
@ -144,51 +131,63 @@ void Module::parseMarkup(const std::string& data) {
Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p;
Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_p("::");
Argument arg0, arg;
Rule namespace_arg_p = namespace_name_p
[push_back_a(arg.type.namespaces)] >> str_p("::");
Rule argEigenType_p =
eigenType_p[assign_a(arg.type)];
eigenType_p[assign_a(arg.type.name)];
Rule eigenRef_p =
!str_p("const") [assign_a(arg.is_const,true)] >>
eigenType_p [assign_a(arg.type)] >>
eigenType_p [assign_a(arg.type.name)] >>
ch_p('&') [assign_a(arg.is_ref,true)];
Rule classArg_p =
!str_p("const") [assign_a(arg.is_const,true)] >>
*namespace_arg_p >>
className_p[assign_a(arg.type)] >>
className_p[assign_a(arg.type.name)] >>
!ch_p('&')[assign_a(arg.is_ref,true)];
Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
// TODO, do we really need cls here? Non-local
Class cls0(verbose),cls(verbose);
Rule classParent_p =
*(namespace_name_p[push_back_a(cls.qualifiedParent)] >> str_p("::")) >>
className_p[push_back_a(cls.qualifiedParent)];
*(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >>
className_p[assign_a(cls.qualifiedParent.name)];
Rule templateInstantiation_p =
(*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >>
className_p[push_back_a(templateInstantiationNamespace)])
[push_back_a(templateInstantiations, templateInstantiationNamespace)]
[clear_a(templateInstantiationNamespace)];
// parse "gtsam::Pose2" and add to templateArgValues
Qualified templateArgValue;
vector<Qualified> templateArgValues;
Rule templateArgValue_p =
(*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >>
className_p[assign_a(templateArgValue.name)])
[push_back_a(templateArgValues, templateArgValue)]
[clear_a(templateArgValue)];
Rule templateInstantiations_p =
// template<CALIBRATION = {gtsam::Cal3DS2}>
string templateArgName;
Rule templateArgValues_p =
(str_p("template") >>
'<' >> name_p[assign_a(templateArgument)] >> '=' >> '{' >>
!(templateInstantiation_p >> *(',' >> templateInstantiation_p)) >>
'}' >> '>')
[push_back_a(cls.templateArgs, templateArgument)];
'<' >> name_p[assign_a(templateArgName)] >> '=' >>
'{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >>
'>');
// parse "gtsam::Pose2" and add to singleInstantiation.typeList
TemplateInstantiationTypedef singleInstantiation;
Rule templateSingleInstantiationArg_p =
(*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >>
className_p[push_back_a(templateInstantiationNamespace)])
[push_back_a(singleInstantiation.typeList, templateInstantiationNamespace)]
[clear_a(templateInstantiationNamespace)];
(*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >>
className_p[assign_a(templateArgValue.name)])
[push_back_a(singleInstantiation.typeList, templateArgValue)]
[clear_a(templateArgValue)];
// typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
TemplateInstantiationTypedef singleInstantiation0;
Rule templateSingleInstantiation_p =
(str_p("typedef") >>
*(namespace_name_p[push_back_a(singleInstantiation.classNamespaces)] >> str_p("::")) >>
className_p[assign_a(singleInstantiation.className)] >>
*(namespace_name_p[push_back_a(singleInstantiation.class_.namespaces)] >> str_p("::")) >>
className_p[assign_a(singleInstantiation.class_.name)] >>
'<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >>
'>' >>
className_p[assign_a(singleInstantiation.name)] >>
@ -197,129 +196,128 @@ void Module::parseMarkup(const std::string& data) {
[push_back_a(templateInstantiationTypedefs, singleInstantiation)]
[assign_a(singleInstantiation, singleInstantiation0)];
// template<POSE, POINT>
Rule templateList_p =
(str_p("template") >>
'<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >>
'>');
// NOTE: allows for pointers to all types
ArgumentList args;
Rule argument_p =
((basisType_p[assign_a(arg.type)] | argEigenType_p | eigenRef_p | classArg_p)
((basisType_p[assign_a(arg.type.name)] | argEigenType_p | eigenRef_p | classArg_p)
>> !ch_p('*')[assign_a(arg.is_ptr,true)]
>> name_p[assign_a(arg.name)])
[push_back_a(args, arg)]
[assign_a(arg,arg0)];
Rule argumentList_p = !argument_p >> * (',' >> argument_p);
// parse class constructor
Constructor constructor0(verbose), constructor(verbose);
Rule constructor_p =
(className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p)
[push_back_a(constructor.args_list, args)]
[assign_a(args,args0)];
//[assign_a(constructor.args,args)]
//[assign_a(constructor.name,cls.name)]
//[push_back_a(cls.constructors, constructor)]
//[assign_a(constructor,constructor0)];
[bl::bind(&Constructor::push_back, bl::var(constructor), bl::var(args))]
[clear_a(args)];
vector<string> namespaces_return; /// namespace for current return type
Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::");
// HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish
static const ReturnValue::return_category RETURN_EIGEN = ReturnValue::EIGEN;
static const ReturnValue::return_category RETURN_BASIS = ReturnValue::BASIS;
static const ReturnValue::return_category RETURN_CLASS = ReturnValue::CLASS;
static const ReturnValue::return_category RETURN_VOID = ReturnValue::VOID;
static const ReturnType::return_category RETURN_EIGEN = ReturnType::EIGEN;
static const ReturnType::return_category RETURN_BASIS = ReturnType::BASIS;
static const ReturnType::return_category RETURN_CLASS = ReturnType::CLASS;
static const ReturnType::return_category RETURN_VOID = ReturnType::VOID;
Rule returnType1_p =
(basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_BASIS)]) |
((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)]
>> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_CLASS)]) >>
!ch_p('*')[assign_a(retVal.isPtr1,true)]) |
(eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_EIGEN)]);
ReturnType retType0, retType;
Rule returnType_p =
(basisType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_BASIS)]) |
((*namespace_ret_p)[assign_a(retType.namespaces, namespaces_return)][clear_a(namespaces_return)]
>> (className_p[assign_a(retType.name)][assign_a(retType.category, RETURN_CLASS)]) >>
!ch_p('*')[assign_a(retType.isPtr,true)]) |
(eigenType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_EIGEN)]);
Rule returnType2_p =
(basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_BASIS)]) |
((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)]
>> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_CLASS)]) >>
!ch_p('*') [assign_a(retVal.isPtr2,true)]) |
(eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_EIGEN)]);
ReturnValue retVal0, retVal;
Rule returnType1_p = returnType_p[assign_a(retVal.type1,retType)][assign_a(retType,retType0)];
Rule returnType2_p = returnType_p[assign_a(retVal.type2,retType)][assign_a(retType,retType0)];
Rule pair_p =
(str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>')
[assign_a(retVal.isPair,true)];
Rule void_p = str_p("void")[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_VOID)];
Rule void_p = str_p("void")[assign_a(retVal.type1.name)][assign_a(retVal.type1.category, RETURN_VOID)];
Rule returnType_p = void_p | pair_p | returnType1_p;
Rule returnValue_p = void_p | pair_p | returnType1_p;
Rule methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
// gtsam::Values retract(const gtsam::VectorValues& delta) const;
string methodName;
bool isConst, isConst0 = false;
Rule method_p =
(returnType_p >> methodName_p[assign_a(methodName)] >>
!templateArgValues_p >>
(returnValue_p >> methodName_p[assign_a(methodName)] >>
'(' >> argumentList_p >> ')' >>
!str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p)
[bl::bind(&Method::addOverload,
bl::var(cls.methods)[bl::var(methodName)],
verbose,
bl::var(isConst),
bl::var(methodName),
bl::var(args),
bl::var(retVal))]
[assign_a(isConst,isConst0)]
[assign_a(methodName,methodName0)]
[assign_a(args,args0)]
[assign_a(retVal,retVal0)];
[bl::bind(&Class::addMethod, bl::var(cls), verbose, bl::var(isConst),
bl::var(methodName), bl::var(args), bl::var(retVal),
bl::var(templateArgName), bl::var(templateArgValues))]
[assign_a(retVal,retVal0)]
[clear_a(args)]
[clear_a(templateArgValues)]
[assign_a(isConst,isConst0)];
Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
Rule static_method_p =
(str_p("static") >> returnType_p >> staticMethodName_p[assign_a(methodName)] >>
(str_p("static") >> returnValue_p >> staticMethodName_p[assign_a(methodName)] >>
'(' >> argumentList_p >> ')' >> ';' >> *comments_p)
[bl::bind(&StaticMethod::addOverload,
bl::var(cls.static_methods)[bl::var(methodName)],
verbose,
bl::var(methodName),
bl::var(args),
bl::var(retVal))]
[assign_a(methodName,methodName0)]
[assign_a(args,args0)]
[assign_a(retVal,retVal0)];
bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)]
[assign_a(retVal,retVal0)]
[clear_a(args)];
Rule functions_p = constructor_p | method_p | static_method_p;
// parse a full class
vector<Qualified> templateInstantiations;
Rule class_p =
(str_p("")[assign_a(cls,cls0)])
>> (!(templateInstantiations_p | templateList_p)
eps_p[assign_a(cls,cls0)]
>> (!(templateArgValues_p
[push_back_a(cls.templateArgs, templateArgName)]
[assign_a(templateInstantiations,templateArgValues)]
[clear_a(templateArgValues)]
| templateList_p)
>> !(str_p("virtual")[assign_a(cls.isVirtual, true)])
>> str_p("class")
>> className_p[assign_a(cls.name)]
>> ((':' >> classParent_p >> '{') | '{')
>> *(functions_p | comments_p)
>> str_p("};"))
[assign_a(constructor.name, cls.name)]
[bl::bind(&Constructor::initializeOrCheck, bl::var(constructor),
bl::var(cls.name), Qualified(), verbose)]
[assign_a(cls.constructor, constructor)]
[assign_a(cls.namespaces, namespaces)]
[assign_a(deconstructor.name,cls.name)]
[assign_a(cls.deconstructor, deconstructor)]
[bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), bl::var(templateArgument), bl::var(templateInstantiations))]
[assign_a(deconstructor,deconstructor0)]
[assign_a(cls.deconstructor.name,cls.name)]
[bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls),
bl::var(templateInstantiations))]
[clear_a(templateInstantiations)]
[assign_a(constructor, constructor0)]
[assign_a(cls,cls0)]
[clear_a(templateArgument)]
[clear_a(templateInstantiations)];
[assign_a(cls,cls0)];
// parse a global function
Qualified globalFunction;
Rule global_function_p =
(returnType_p >> staticMethodName_p[assign_a(methodName)] >>
(returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >>
'(' >> argumentList_p >> ')' >> ';' >> *comments_p)
[assign_a(globalFunction.namespaces,namespaces)]
[bl::bind(&GlobalFunction::addOverload,
bl::var(global_functions)[bl::var(methodName)],
verbose,
bl::var(methodName),
bl::var(args),
bl::var(retVal),
bl::var(namespaces))]
[assign_a(methodName,methodName0)]
[assign_a(args,args0)]
[assign_a(retVal,retVal0)];
bl::var(global_functions)[bl::var(globalFunction.name)],
bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)]
[assign_a(retVal,retVal0)]
[clear_a(globalFunction)]
[clear_a(args)];
Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>');
@ -327,19 +325,21 @@ void Module::parseMarkup(const std::string& data) {
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wuninitialized"
#endif
Rule namespace_def_p =
(str_p("namespace")
>> namespace_name_p[push_back_a(namespaces)]
>> ch_p('{')
>> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p)
>> ch_p('}'))
[pop_a(namespaces)];
Rule namespace_def_p =
(str_p("namespace")
>> namespace_name_p[push_back_a(namespaces)]
>> ch_p('{')
>> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p)
>> ch_p('}'))
[pop_a(namespaces)];
#ifdef __clang__
#pragma clang diagnostic pop
#endif
// parse forward declaration
ForwardDeclaration fwDec0, fwDec;
Rule forward_declaration_p =
!(str_p("virtual")[assign_a(fwDec.isVirtual, true)])
>> str_p("class")
@ -367,7 +367,7 @@ void Module::parseMarkup(const std::string& data) {
BOOST_SPIRIT_DEBUG_NODE(returnType2_p);
BOOST_SPIRIT_DEBUG_NODE(pair_p);
BOOST_SPIRIT_DEBUG_NODE(void_p);
BOOST_SPIRIT_DEBUG_NODE(returnType_p);
BOOST_SPIRIT_DEBUG_NODE(returnValue_p);
BOOST_SPIRIT_DEBUG_NODE(methodName_p);
BOOST_SPIRIT_DEBUG_NODE(method_p);
BOOST_SPIRIT_DEBUG_NODE(class_p);
@ -388,125 +388,41 @@ void Module::parseMarkup(const std::string& data) {
}
// Post-process classes for serialization markers
BOOST_FOREACH(Class& cls, classes) {
Class::Methods::iterator serializable_it = cls.methods.find("serializable");
if (serializable_it != cls.methods.end()) {
#ifndef WRAP_DISABLE_SERIALIZE
cls.isSerializable = true;
#else
cout << "Ignoring serializable() flag in class " << cls.name << endl;
#endif
cls.methods.erase(serializable_it);
}
Class::Methods::iterator serialize_it = cls.methods.find("serialize");
if (serialize_it != cls.methods.end()) {
#ifndef WRAP_DISABLE_SERIALIZE
cls.isSerializable = true;
cls.hasSerialization= true;
#else
cout << "Ignoring serialize() flag in class " << cls.name << endl;
#endif
cls.methods.erase(serialize_it);
}
}
BOOST_FOREACH(Class& cls, classes)
cls.erase_serialization();
// Explicitly add methods to the classes from parents so it shows in documentation
BOOST_FOREACH(Class& cls, classes)
{
map<string, Method> inhereted = appendInheretedMethods(cls, classes);
cls.methods.insert(inhereted.begin(), inhereted.end());
}
}
/* ************************************************************************* */
template<class T>
void verifyArguments(const vector<string>& validArgs, const map<string,T>& vt) {
typedef typename map<string,T>::value_type Name_Method;
BOOST_FOREACH(const Name_Method& name_method, vt) {
const T& t = name_method.second;
BOOST_FOREACH(const ArgumentList& argList, t.argLists) {
BOOST_FOREACH(Argument arg, argList) {
string fullType = arg.qualifiedType("::");
if(find(validArgs.begin(), validArgs.end(), fullType)
== validArgs.end())
throw DependencyMissing(fullType, t.name);
}
}
}
}
/* ************************************************************************* */
template<class T>
void verifyReturnTypes(const vector<string>& validtypes, const map<string,T>& vt) {
typedef typename map<string,T>::value_type Name_Method;
BOOST_FOREACH(const Name_Method& name_method, vt) {
const T& t = name_method.second;
BOOST_FOREACH(const ReturnValue& retval, t.returnVals) {
if (find(validtypes.begin(), validtypes.end(), retval.qualifiedType1("::")) == validtypes.end())
throw DependencyMissing(retval.qualifiedType1("::"), t.name);
if (retval.isPair && find(validtypes.begin(), validtypes.end(), retval.qualifiedType2("::")) == validtypes.end())
throw DependencyMissing(retval.qualifiedType2("::"), t.name);
}
}
}
/* ************************************************************************* */
void Module::generateIncludes(FileWriter& file) const {
// collect includes
vector<string> all_includes(includes);
// sort and remove duplicates
sort(all_includes.begin(), all_includes.end());
vector<string>::const_iterator last_include = unique(all_includes.begin(), all_includes.end());
vector<string>::const_iterator it = all_includes.begin();
// add includes to file
for (; it != last_include; ++it)
file.oss << "#include <" << *it << ">" << endl;
file.oss << "\n";
}
/* ************************************************************************* */
void Module::matlab_code(const string& toolboxPath, const string& headerPath) const {
fs::create_directories(toolboxPath);
cls.appendInheritedMethods(cls, classes);
// Expand templates - This is done first so that template instantiations are
// counted in the list of valid types, have their attributes and dependencies
// checked, etc.
vector<Class> expandedClasses = ExpandTypedefInstantiations(classes, templateInstantiationTypedefs);
expandedClasses = ExpandTypedefInstantiations(classes,
templateInstantiationTypedefs);
// Dependency check list
vector<string> validTypes = GenerateValidTypes(expandedClasses, forward_declarations);
vector<string> validTypes = GenerateValidTypes(expandedClasses,
forward_declarations);
// Check that all classes have been defined somewhere
verifyArguments<GlobalFunction>(validTypes, global_functions);
verifyReturnTypes<GlobalFunction>(validTypes, global_functions);
bool hasSerialiable = false;
BOOST_FOREACH(const Class& cls, expandedClasses) {
hasSerialiable |= cls.isSerializable;
// verify all of the function arguments
//TODO:verifyArguments<ArgumentList>(validTypes, cls.constructor.args_list);
verifyArguments<StaticMethod>(validTypes, cls.static_methods);
verifyArguments<Method>(validTypes, cls.methods);
// verify function return types
verifyReturnTypes<StaticMethod>(validTypes, cls.static_methods);
verifyReturnTypes<Method>(validTypes, cls.methods);
// verify parents
if(!cls.qualifiedParent.empty() && std::find(validTypes.begin(), validTypes.end(), wrap::qualifiedName("::", cls.qualifiedParent)) == validTypes.end())
throw DependencyMissing(wrap::qualifiedName("::", cls.qualifiedParent), cls.qualifiedName("::"));
}
hasSerialiable = false;
BOOST_FOREACH(const Class& cls, expandedClasses)
cls.verifyAll(validTypes,hasSerialiable);
// Create type attributes table and check validity
TypeAttributesTable typeAttributes;
typeAttributes.addClasses(expandedClasses);
typeAttributes.addForwardDeclarations(forward_declarations);
typeAttributes.checkValidity(expandedClasses);
}
/* ************************************************************************* */
void Module::matlab_code(const string& toolboxPath) const {
fs::create_directories(toolboxPath);
// create the unified .cpp switch file
const string wrapperName = name + "_wrapper";
@ -527,19 +443,18 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
// Generate includes while avoiding redundant includes
generateIncludes(wrapperFile);
// create typedef classes - we put this at the top of the wrap file so that collectors and method arguments can use these typedefs
BOOST_FOREACH(const Class& cls, expandedClasses) {
// create typedef classes - we put this at the top of the wrap file so that
// collectors and method arguments can use these typedefs
BOOST_FOREACH(const Class& cls, expandedClasses)
if(!cls.typedefName.empty())
wrapperFile.oss << cls.getTypedef() << "\n";
}
wrapperFile.oss << "\n";
// Generate boost.serialization export flags (needs typedefs from above)
if (hasSerialiable) {
BOOST_FOREACH(const Class& cls, expandedClasses) {
BOOST_FOREACH(const Class& cls, expandedClasses)
if(cls.isSerializable)
wrapperFile.oss << cls.getSerializationExport() << "\n";
}
wrapperFile.oss << "\n";
}
@ -552,14 +467,12 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
vector<string> functionNames; // Function names stored by index for switch
// create proxy class and wrapper code
BOOST_FOREACH(const Class& cls, expandedClasses) {
BOOST_FOREACH(const Class& cls, expandedClasses)
cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
}
// create matlab files and wrapper code for global functions
BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions) {
BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions)
p.second.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
}
// finish wrapper file
wrapperFile.oss << "\n";
@ -567,29 +480,25 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
wrapperFile.emit(true);
}
/* ************************************************************************* */
map<string, Method> Module::appendInheretedMethods(const Class& cls, const vector<Class>& classes)
{
map<string, Method> methods;
if(!cls.qualifiedParent.empty())
{
//Find Class
BOOST_FOREACH(const Class& parent, classes) {
//We found the class for our parent
if(parent.name == cls.qualifiedParent.back())
{
Methods inhereted = appendInheretedMethods(parent, classes);
methods.insert(inhereted.begin(), inhereted.end());
}
}
} else {
methods.insert(cls.methods.begin(), cls.methods.end());
}
return methods;
}
/* ************************************************************************* */
void Module::generateIncludes(FileWriter& file) const {
// collect includes
vector<string> all_includes(includes);
// sort and remove duplicates
sort(all_includes.begin(), all_includes.end());
vector<string>::const_iterator last_include = unique(all_includes.begin(), all_includes.end());
vector<string>::const_iterator it = all_includes.begin();
// add includes to file
for (; it != last_include; ++it)
file.oss << "#include <" << *it << ">" << endl;
file.oss << "\n";
}
/* ************************************************************************* */
void Module::finish_wrapper(FileWriter& file, const std::vector<std::string>& functionNames) const {
file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
file.oss << "{\n";
@ -741,3 +650,31 @@ void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& modul
}
/* ************************************************************************* */
void Module::python_wrapper(const string& toolboxPath) const {
fs::create_directories(toolboxPath);
// create the unified .cpp switch file
const string wrapperName = name + "_python";
string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
FileWriter wrapperFile(wrapperFileName, verbose, "//");
wrapperFile.oss << "#include <boost/python.hpp>\n\n";
wrapperFile.oss << "using namespace boost::python;\n";
wrapperFile.oss << "BOOST_PYTHON_MODULE(" + name + ")\n";
wrapperFile.oss << "{\n";
// write out classes
BOOST_FOREACH(const Class& cls, expandedClasses)
cls.python_wrapper(wrapperFile);
// write out global functions
BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions)
p.second.python_wrapper(wrapperFile);
// finish wrapper file
wrapperFile.oss << "}\n";
wrapperFile.emit(true);
}
/* ************************************************************************* */

56
wrap/Module.h
View File

@ -37,43 +37,53 @@ struct Module {
typedef std::map<std::string, GlobalFunction> GlobalFunctions;
typedef std::map<std::string, Method> Methods;
std::string name; ///< module name
bool verbose; ///< verbose flag
// Filled during parsing:
std::string name; ///< module name
bool verbose; ///< verbose flag
std::vector<Class> classes; ///< list of classes
std::vector<TemplateInstantiationTypedef> templateInstantiationTypedefs; ///< list of template instantiations
std::vector<ForwardDeclaration> forward_declarations;
std::vector<std::string> includes; ///< Include statements
std::vector<std::string> includes; ///< Include statements
GlobalFunctions global_functions;
// After parsing:
std::vector<Class> expandedClasses;
bool hasSerialiable;
TypeAttributesTable typeAttributes;
/// constructor that parses interface file
Module(const std::string& interfacePath,
const std::string& moduleName,
bool enable_verbose=true);
Module(const std::string& interfacePath, const std::string& moduleName,
bool enable_verbose = true);
/// Dummy constructor that does no parsing - use only for testing
Module(const std::string& moduleName, bool enable_verbose=true);
//Recursive method to append all methods inhereted from parent classes
std::map<std::string, Method> appendInheretedMethods(const Class& cls, const std::vector<Class>& classes);
/// MATLAB code generation:
void matlab_code(
const std::string& path,
const std::string& headerPath) const; // FIXME: headerPath not actually used?
void finish_wrapper(FileWriter& file, const std::vector<std::string>& functionNames) const;
void generateIncludes(FileWriter& file) const;
Module(const std::string& moduleName, bool enable_verbose = true);
/// non-const function that performs parsing - typically called by constructor
/// Throws exception on failure
void parseMarkup(const std::string& data);
/// MATLAB code generation:
void matlab_code(const std::string& path) const;
void generateIncludes(FileWriter& file) const;
void finish_wrapper(FileWriter& file,
const std::vector<std::string>& functionNames) const;
/// Python code generation:
void python_wrapper(const std::string& path) const;
private:
static std::vector<Class> ExpandTypedefInstantiations(const std::vector<Class>& classes, const std::vector<TemplateInstantiationTypedef> instantiations);
static std::vector<std::string> GenerateValidTypes(const std::vector<Class>& classes, const std::vector<ForwardDeclaration> forwardDeclarations);
static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes);
static void WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes);
static std::vector<Class> ExpandTypedefInstantiations(
const std::vector<Class>& classes,
const std::vector<TemplateInstantiationTypedef> instantiations);
static std::vector<std::string> GenerateValidTypes(
const std::vector<Class>& classes,
const std::vector<ForwardDeclaration> forwardDeclarations);
static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile,
const std::string& moduleName, const std::vector<Class>& classes);
static void WriteRTTIRegistry(FileWriter& wrapperFile,
const std::string& moduleName, const std::vector<Class>& classes);
};
} // \namespace wrap

126
wrap/OverloadedFunction.h Normal file
View File

@ -0,0 +1,126 @@
/* ----------------------------------------------------------------------------
* 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 OverloadedFunction.h
* @brief Function that can overload its arguments only
* @author Frank Dellaert
* @date Nov 13, 2014
**/
#pragma once
#include "Function.h"
#include "Argument.h"
namespace wrap {
/**
* ArgumentList Overloads
*/
class ArgumentOverloads {
protected:
std::vector<ArgumentList> argLists_;
public:
size_t nrOverloads() const {
return argLists_.size();
}
const ArgumentList& argumentList(size_t i) const {
return argLists_.at(i);
}
void push_back(const ArgumentList& args) {
argLists_.push_back(args);
}
std::vector<ArgumentList> expandArgumentListsTemplate(
const TemplateSubstitution& ts) const {
std::vector<ArgumentList> result;
BOOST_FOREACH(const ArgumentList& argList, argLists_) {
ArgumentList instArgList = argList.expandTemplate(ts);
result.push_back(instArgList);
}
return result;
}
/// Expand templates, imperative !
virtual void ExpandTemplate(const TemplateSubstitution& ts) {
argLists_ = expandArgumentListsTemplate(ts);
}
void verifyArguments(const std::vector<std::string>& validArgs,
const std::string s) const {
BOOST_FOREACH(const ArgumentList& argList, argLists_) {
BOOST_FOREACH(Argument arg, argList) {
std::string fullType = arg.type.qualifiedName("::");
if (find(validArgs.begin(), validArgs.end(), fullType)
== validArgs.end())
throw DependencyMissing(fullType, "checking argument of " + s);
}
}
}
friend std::ostream& operator<<(std::ostream& os,
const ArgumentOverloads& overloads) {
BOOST_FOREACH(const ArgumentList& argList, overloads.argLists_)
os << argList << std::endl;
return os;
}
};
class OverloadedFunction: public Function, public ArgumentOverloads {
public:
bool addOverload(const std::string& name, const ArgumentList& args,
const Qualified& instName = Qualified(), bool verbose = false) {
bool first = initializeOrCheck(name, instName, verbose);
ArgumentOverloads::push_back(args);
return first;
}
private:
};
// Templated checking functions
// TODO: do this via polymorphism, use transform ?
template<class F>
static std::map<std::string, F> expandMethodTemplate(
const std::map<std::string, F>& methods, const TemplateSubstitution& ts) {
std::map<std::string, F> result;
typedef std::pair<const std::string, F> NamedMethod;
BOOST_FOREACH(NamedMethod namedMethod, methods) {
F instMethod = namedMethod.second;
instMethod.expandTemplate(ts);
namedMethod.second = instMethod;
result.insert(namedMethod);
}
return result;
}
template<class F>
inline void verifyArguments(const std::vector<std::string>& validArgs,
const std::map<std::string, F>& vt) {
typedef typename std::map<std::string, F>::value_type NamedMethod;
BOOST_FOREACH(const NamedMethod& namedMethod, vt)
namedMethod.second.verifyArguments(validArgs);
}
} // \namespace wrap

77
wrap/Qualified.h Normal file
View File

@ -0,0 +1,77 @@
/* ----------------------------------------------------------------------------
* 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 Qualified.h
* @brief Qualified name
* @author Frank Dellaert
* @date Nov 11, 2014
**/
#pragma once
#include <string>
#include <vector>
namespace wrap {
/**
* Class to encapuslate a qualified name, i.e., with (nested) namespaces
*/
struct Qualified {
std::vector<std::string> namespaces; ///< Stack of namespaces
std::string name; ///< type name
Qualified(const std::string& name_ = "") :
name(name_) {
}
bool empty() const {
return namespaces.empty() && name.empty();
}
void clear() {
namespaces.clear();
name.clear();
}
bool operator!=(const Qualified& other) const {
return other.name != name || other.namespaces != namespaces;
}
/// Return a qualified string using given delimiter
std::string qualifiedName(const std::string& delimiter = "") const {
std::string result;
for (std::size_t i = 0; i < namespaces.size(); ++i)
result += (namespaces[i] + delimiter);
result += name;
return result;
}
/// Return a matlab file name, i.e. "toolboxPath/+ns1/+ns2/name.m"
std::string matlabName(const std::string& toolboxPath) const {
std::string result = toolboxPath;
for (std::size_t i = 0; i < namespaces.size(); ++i)
result += ("/+" + namespaces[i]);
result += "/" + name + ".m";
return result;
}
friend std::ostream& operator<<(std::ostream& os, const Qualified& q) {
os << q.qualifiedName("::");
return os;
}
};
} // \namespace wrap

61
wrap/ReturnType.cpp Normal file
View File

@ -0,0 +1,61 @@
/**
* @file ReturnType.cpp
* @date Nov 13, 2014
* @author Frank Dellaert
*/
#include "ReturnType.h"
#include "utilities.h"
#include <boost/foreach.hpp>
#include <iostream>
using namespace std;
using namespace wrap;
/* ************************************************************************* */
string ReturnType::str(bool add_ptr) const {
return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name);
}
/* ************************************************************************* */
void ReturnType::wrap_result(const string& out, const string& result,
FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const {
string cppType = qualifiedName("::"), matlabType = qualifiedName(".");
if (category == CLASS) {
string objCopy, ptrType;
ptrType = "Shared" + name;
const bool isVirtual = typeAttributes.at(cppType).isVirtual;
if (isVirtual) {
if (isPtr)
objCopy = result;
else
objCopy = result + ".clone()";
} else {
if (isPtr)
objCopy = result;
else
objCopy = ptrType + "(new " + cppType + "(" + result + "))";
}
wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\""
<< matlabType << "\", " << (isVirtual ? "true" : "false") << ");\n";
} else if (isPtr) {
wrapperFile.oss << " Shared" << name << "* ret = new Shared" << name << "("
<< result << ");" << endl;
wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType
<< "\");\n";
} else if (matlabType != "void")
wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result
<< ");\n";
}
/* ************************************************************************* */
void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const {
if (category == CLASS)
wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::")
<< "> Shared" << name << ";" << endl;
}
/* ************************************************************************* */

67
wrap/ReturnType.h Normal file
View File

@ -0,0 +1,67 @@
/**
* @file ReturnValue.h
* @brief Encapsulates a return type of a method
* @date Nov 13, 2014
* @author Frank Dellaert
*/
#include "Qualified.h"
#include "FileWriter.h"
#include "TypeAttributesTable.h"
#include "utilities.h"
#pragma once
namespace wrap {
/**
* Encapsulates return value of a method or function
*/
struct ReturnType: Qualified {
/// the different supported return value categories
typedef enum {
CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
} return_category;
bool isPtr;
return_category category;
ReturnType() :
isPtr(false), category(CLASS) {
}
ReturnType(const std::string& name) :
isPtr(false), category(CLASS) {
Qualified::name = name;
}
void rename(const Qualified& q) {
name = q.name;
namespaces = q.namespaces;
}
/// Check if this type is in a set of valid types
template<class TYPES>
void verify(TYPES validtypes, const std::string& s) const {
std::string key = qualifiedName("::");
if (find(validtypes.begin(), validtypes.end(), key) == validtypes.end())
throw DependencyMissing(key, "checking return type of " + s);
}
private:
friend struct ReturnValue;
std::string str(bool add_ptr) const;
/// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
void wrap_result(const std::string& out, const std::string& result,
FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const;
/// Creates typedef
void wrapTypeUnwrap(FileWriter& wrapperFile) const;
};
} // \namespace wrap

151
wrap/ReturnValue.cpp
View File

@ -1,153 +1,72 @@
/**
* @file ReturnValue.cpp
*
* @date Dec 1, 2011
* @author Alex Cunningham
* @author Andrew Melim
* @author Richard Roberts
*/
#include <boost/foreach.hpp>
#include "ReturnValue.h"
#include "utilities.h"
#include <boost/foreach.hpp>
#include <iostream>
using namespace std;
using namespace wrap;
/* ************************************************************************* */
string ReturnValue::return_type(bool add_ptr, pairing p) const {
if (p==pair && isPair) {
string str = "pair< " +
maybe_shared_ptr(add_ptr || isPtr1, qualifiedType1("::"), type1) + ", " +
maybe_shared_ptr(add_ptr || isPtr2, qualifiedType2("::"), type2) + " >";
return str;
} else
return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"), (p==arg2)? type2 : type1);
ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const {
ReturnValue instRetVal = *this;
instRetVal.type1 = ts(type1);
if (isPair)
instRetVal.type2 = ts(type2);
return instRetVal;
}
/* ************************************************************************* */
string ReturnValue::return_type(bool add_ptr) const {
if (isPair)
return "pair< " + type1.str(add_ptr) + ", " + type2.str(add_ptr) + " >";
else
return type1.str(add_ptr);
}
/* ************************************************************************* */
string ReturnValue::matlab_returnType() const {
return isPair? "[first,second]" : "result";
return isPair ? "[first,second]" : "result";
}
/* ************************************************************************* */
string ReturnValue::qualifiedType1(const string& delim) const {
string result;
BOOST_FOREACH(const string& ns, namespaces1) result += ns + delim;
return result + type1;
}
/* ************************************************************************* */
string ReturnValue::qualifiedType2(const string& delim) const {
string result;
BOOST_FOREACH(const string& ns, namespaces2) result += ns + delim;
return result + type2;
}
/* ************************************************************************* */
//TODO:Fix this
void ReturnValue::wrap_result(const string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const {
string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1(".");
string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(".");
void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile,
const TypeAttributesTable& typeAttributes) const {
if (isPair) {
// For a pair, store the returned pair so we do not evaluate the function twice
file.oss << " " << return_type(false, pair) << " pairResult = " << result << ";\n";
// first return value in pair
if (category1 == ReturnValue::CLASS) { // if we are going to make one
string objCopy, ptrType;
ptrType = "Shared" + type1;
const bool isVirtual = typeAttributes.at(cppType1).isVirtual;
if(isVirtual) {
if(isPtr1)
objCopy = "pairResult.first";
else
objCopy = "pairResult.first.clone()";
} else {
if(isPtr1)
objCopy = "pairResult.first";
else
objCopy = ptrType + "(new " + cppType1 + "(pairResult.first))";
}
file.oss << " out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n";
} else if(isPtr1) {
file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(pairResult.first);" << endl;
file.oss << " out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\", false);\n";
} else // if basis type
file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(pairResult.first);\n";
// second return value in pair
if (category2 == ReturnValue::CLASS) { // if we are going to make one
string objCopy, ptrType;
ptrType = "Shared" + type2;
const bool isVirtual = typeAttributes.at(cppType2).isVirtual;
if(isVirtual) {
if(isPtr2)
objCopy = "pairResult.second";
else
objCopy = "pairResult.second.clone()";
} else {
if(isPtr2)
objCopy = "pairResult.second";
else
objCopy = ptrType + "(new " + cppType2 + "(pairResult.second))";
}
file.oss << " out[1] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType2 << "\", " << (isVirtual ? "true" : "false") << ");\n";
} else if(isPtr2) {
file.oss << " Shared" << type2 <<"* ret = new Shared" << type2 << "(pairResult.second);" << endl;
file.oss << " out[1] = wrap_shared_ptr(ret,\"" << matlabType2 << "\");\n";
} else
file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(pairResult.second);\n";
wrapperFile.oss << " " << return_type(true) << " pairResult = " << result
<< ";\n";
type1.wrap_result(" out[0]", "pairResult.first", wrapperFile,
typeAttributes);
type2.wrap_result(" out[1]", "pairResult.second", wrapperFile,
typeAttributes);
} else { // Not a pair
if (category1 == ReturnValue::CLASS) {
string objCopy, ptrType;
ptrType = "Shared" + type1;
const bool isVirtual = typeAttributes.at(cppType1).isVirtual;
if(isVirtual) {
if(isPtr1)
objCopy = result;
else
objCopy = result + ".clone()";
} else {
if(isPtr1)
objCopy = result;
else
objCopy = ptrType + "(new " + cppType1 + "(" + result + "))";
}
file.oss << " out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n";
} else if(isPtr1) {
file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(" << result << ");" << endl;
file.oss << " out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\");\n";
} else if (matlabType1!="void")
file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(" << result << ");\n";
type1.wrap_result(" out[0]", result, wrapperFile, typeAttributes);
}
}
/* ************************************************************************* */
void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const {
if(isPair)
{
if(category1 == ReturnValue::CLASS)
wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType1("::") << "> Shared" << type1 << ";"<< endl;
if(category2 == ReturnValue::CLASS)
wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType2("::") << "> Shared" << type2 << ";"<< endl;
}
else {
if (category1 == ReturnValue::CLASS)
wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType1("::") << "> Shared" << type1 << ";"<< endl;
}
type1.wrapTypeUnwrap(wrapperFile);
if (isPair)
type2.wrapTypeUnwrap(wrapperFile);
}
/* ************************************************************************* */
void ReturnValue::emit_matlab(FileWriter& file) const {
void ReturnValue::emit_matlab(FileWriter& proxyFile) const {
string output;
if (isPair)
file.oss << "[ varargout{1} varargout{2} ] = ";
else if (category1 != ReturnValue::VOID)
file.oss << "varargout{1} = ";
proxyFile.oss << "[ varargout{1} varargout{2} ] = ";
else if (type1.category != ReturnType::VOID)
proxyFile.oss << "varargout{1} = ";
}
/* ************************************************************************* */

48
wrap/ReturnValue.h
View File

@ -2,59 +2,61 @@
* @file ReturnValue.h
*
* @brief Encapsulates a return value from a method
*
* @date Dec 1, 2011
* @author Alex Cunningham
* @author Richard Roberts
*/
#include "ReturnType.h"
#include "TemplateSubstitution.h"
#include "FileWriter.h"
#include "TypeAttributesTable.h"
#include <vector>
#include "utilities.h"
#pragma once
namespace wrap {
/**
* Encapsulates return value of a method or function
* Encapsulates return type of a method or function, possibly a pair
*/
struct ReturnValue {
/// the different supported return value categories
typedef enum {
CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
} return_category;
bool isPtr1, isPtr2, isPair;
return_category category1, category2;
std::string type1, type2;
std::vector<std::string> namespaces1, namespaces2;
bool isPair;
ReturnType type1, type2;
/// Constructor
ReturnValue() :
isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2(
CLASS) {
isPair(false) {
}
typedef enum {
arg1, arg2, pair
} pairing;
/// Constructor
ReturnValue(const ReturnType& type) :
isPair(false), type1(type) {
}
std::string return_type(bool add_ptr, pairing p) const;
/// Substitute template argument
ReturnValue expandTemplate(const TemplateSubstitution& ts) const;
std::string qualifiedType1(const std::string& delim = "") const;
std::string qualifiedType2(const std::string& delim = "") const;
std::string return_type(bool add_ptr) const;
std::string matlab_returnType() const;
void wrap_result(const std::string& result, FileWriter& file,
void wrap_result(const std::string& result, FileWriter& wrapperFile,
const TypeAttributesTable& typeAttributes) const;
void wrapTypeUnwrap(FileWriter& wrapperFile) const;
void emit_matlab(FileWriter& file) const;
void emit_matlab(FileWriter& proxyFile) const;
friend std::ostream& operator<<(std::ostream& os, const ReturnValue& r) {
if (!r.isPair && r.type1.category == ReturnType::VOID)
os << "void";
else
os << r.return_type(true);
return os;
}
};
} // \namespace wrap

172
wrap/StaticMethod.cpp
View File

@ -16,7 +16,7 @@
* @author Richard Roberts
**/
#include "StaticMethod.h"
#include "Method.h"
#include "utilities.h"
#include <boost/foreach.hpp>
@ -30,117 +30,145 @@ using namespace std;
using namespace wrap;
/* ************************************************************************* */
void StaticMethod::addOverload(bool verbose, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal) {
this->verbose = verbose;
this->name = name;
this->argLists.push_back(args);
this->returnVals.push_back(retVal);
void StaticMethod::proxy_header(FileWriter& proxyFile) const {
string upperName = matlabName();
upperName[0] = toupper(upperName[0], locale());
proxyFile.oss << " function varargout = " << upperName << "(varargin)\n";
}
/* ************************************************************************* */
void StaticMethod::proxy_wrapper_fragments(FileWriter& file,
FileWriter& wrapperFile, const string& cppClassName,
const std::string& matlabQualName, const std::string& matlabUniqueName,
const string& wrapperName, const TypeAttributesTable& typeAttributes,
void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, Str cppClassName, Str matlabQualName,
Str matlabUniqueName, Str wrapperName,
const TypeAttributesTable& typeAttributes,
vector<string>& functionNames) const {
string upperName = name;
upperName[0] = std::toupper(upperName[0], std::locale());
// emit header, e.g., function varargout = templatedMethod(this, varargin)
proxy_header(proxyFile);
file.oss << " function varargout = " << upperName << "(varargin)\n";
//Comments for documentation
string up_name = boost::to_upper_copy(name);
file.oss << " % " << up_name << " usage: ";
unsigned int argLCount = 0;
BOOST_FOREACH(ArgumentList argList, argLists) {
argList.emit_prototype(file, name);
if (argLCount != argLists.size() - 1)
file.oss << ", ";
else
file.oss << " : returns "
<< returnVals[0].return_type(false, returnVals[0].pair) << endl;
argLCount++;
}
// Emit comments for documentation
string up_name = boost::to_upper_copy(matlabName());
proxyFile.oss << " % " << up_name << " usage: ";
usage_fragment(proxyFile, matlabName());
file.oss << " % "
// Emit URL to Doxygen page
proxyFile.oss << " % "
<< "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
<< endl;
file.oss << " % " << "" << endl;
file.oss << " % " << "Usage" << endl;
BOOST_FOREACH(ArgumentList argList, argLists) {
file.oss << " % ";
argList.emit_prototype(file, up_name);
file.oss << endl;
}
// Check arguments for all overloads
for (size_t overload = 0; overload < argLists.size(); ++overload) {
// Handle special case of single overload with all numeric arguments
if (nrOverloads() == 1 && argumentList(0).allScalar()) {
// Output proxy matlab code
file.oss << " " << (overload == 0 ? "" : "else");
// TODO: document why is it OK to not check arguments in this case
proxyFile.oss << " ";
const int id = (int) functionNames.size();
argLists[overload].emit_conditional_call(file, returnVals[overload],
wrapperName, id, true); // last bool is to indicate static !
argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id,
isStatic());
// Output C++ wrapper code
const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
matlabUniqueName, (int) overload, id, typeAttributes);
matlabUniqueName, 0, id, typeAttributes, templateArgValue_);
// Add to function list
functionNames.push_back(wrapFunctionName);
}
file.oss << " else\n";
file.oss << " error('Arguments do not match any overload of function "
<< matlabQualName << "." << upperName << "');" << endl;
file.oss << " end\n";
} else {
// Check arguments for all overloads
for (size_t i = 0; i < nrOverloads(); ++i) {
file.oss << " end\n";
// Output proxy matlab code
proxyFile.oss << " " << (i == 0 ? "" : "else");
const int id = (int) functionNames.size();
argumentList(i).emit_conditional_call(proxyFile, returnValue(i),
wrapperName, id, isStatic());
// Output C++ wrapper code
const string wrapFunctionName = wrapper_fragment(wrapperFile,
cppClassName, matlabUniqueName, i, id, typeAttributes,
templateArgValue_);
// Add to function list
functionNames.push_back(wrapFunctionName);
}
proxyFile.oss << " else\n";
proxyFile.oss
<< " error('Arguments do not match any overload of function "
<< matlabQualName << "." << name_ << "');" << endl;
proxyFile.oss << " end\n";
}
proxyFile.oss << " end\n";
}
/* ************************************************************************* */
string StaticMethod::wrapper_fragment(FileWriter& file,
const string& cppClassName, const string& matlabUniqueName, int overload,
int id, const TypeAttributesTable& typeAttributes) const {
string StaticMethod::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
Str matlabUniqueName, int overload, int id,
const TypeAttributesTable& typeAttributes,
const Qualified& instName) const {
// generate code
const string wrapFunctionName = matlabUniqueName + "_" + name + "_"
const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_"
+ boost::lexical_cast<string>(id);
const ArgumentList& args = argLists[overload];
const ReturnValue& returnVal = returnVals[overload];
const ArgumentList& args = argumentList(overload);
const ReturnValue& returnVal = returnValue(overload);
// call
file.oss << "void " << wrapFunctionName
wrapperFile.oss << "void " << wrapFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
// start
file.oss << "{\n";
wrapperFile.oss << "{\n";
returnVal.wrapTypeUnwrap(file);
returnVal.wrapTypeUnwrap(wrapperFile);
file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;"
<< endl;
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName
<< "> Shared;" << endl;
// check arguments
// NOTE: for static functions, there is no object passed
file.oss << " checkArguments(\"" << matlabUniqueName << "." << name
<< "\",nargout,nargin," << args.size() << ");\n";
// get call
// for static methods: cppClassName::staticMethod<TemplateVal>
// for instance methods: obj->instanceMethod<TemplateVal>
string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName,
args, returnVal, typeAttributes, instName);
// unwrap arguments, see Argument.cpp
args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
// call method with default type and wrap result
if (returnVal.type1 != "void")
returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")",
file, typeAttributes);
expanded += ("(" + args.names() + ")");
if (returnVal.type1.name != "void")
returnVal.wrap_result(expanded, wrapperFile, typeAttributes);
else
file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n";
wrapperFile.oss << " " + expanded + ";\n";
// finish
file.oss << "}\n";
wrapperFile.oss << "}\n";
return wrapFunctionName;
}
/* ************************************************************************* */
string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
Str matlabUniqueName, const ArgumentList& args,
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
const Qualified& instName) const {
// check arguments
// NOTE: for static functions, there is no object passed
wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "." << name_
<< "\",nargout,nargin," << args.size() << ");\n";
// unwrap arguments, see Argument.cpp
args.matlab_unwrap(wrapperFile, 0); // We start at 0 because there is no self object
// call method and wrap result
// example: out[0]=wrap<bool>(staticMethod(t));
string expanded = cppClassName + "::" + name_;
if (!instName.empty())
expanded += ("<" + instName.qualifiedName("::") + ">");
return expanded;
}
/* ************************************************************************* */
void StaticMethod::python_wrapper(FileWriter& wrapperFile,
Str className) const {
wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_
<< ");\n";
}
/* ************************************************************************* */

65
wrap/StaticMethod.h
View File

@ -19,44 +19,61 @@
#pragma once
#include "Argument.h"
#include "ReturnValue.h"
#include "TypeAttributesTable.h"
#include "FullyOverloadedFunction.h"
namespace wrap {
/// StaticMethod class
struct StaticMethod {
struct StaticMethod: public FullyOverloadedFunction {
/// Constructor creates empty object
StaticMethod(bool verbosity = true) :
verbose(verbosity) {
typedef const std::string& Str;
virtual bool isStatic() const {
return true;
}
// Then the instance variables are set directly by the Module constructor
bool verbose;
std::string name;
std::vector<ArgumentList> argLists;
std::vector<ReturnValue> returnVals;
// emit a list of comments, one for each overload
void comment_fragment(FileWriter& proxyFile) const {
SignatureOverloads::comment_fragment(proxyFile, matlabName());
}
// The first time this function is called, it initializes the class members
// with those in rhs, but in subsequent calls it adds additional argument
// lists as function overloads.
void addOverload(bool verbose, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal);
void verifyArguments(const std::vector<std::string>& validArgs) const {
SignatureOverloads::verifyArguments(validArgs, name_);
}
void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
SignatureOverloads::verifyReturnTypes(validtypes, name_);
}
// MATLAB code generation
// classPath is class directory, e.g., ../matlab/@Point2
void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& cppClassName, const std::string& matlabQualName,
const std::string& matlabUniqueName, const std::string& wrapperName,
const TypeAttributesTable& typeAttributes,
Str cppClassName, Str matlabQualName, Str matlabUniqueName,
Str wrapperName, const TypeAttributesTable& typeAttributes,
std::vector<std::string>& functionNames) const;
private:
std::string wrapper_fragment(FileWriter& file,
const std::string& cppClassName, const std::string& matlabUniqueName,
int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
// emit python wrapper
void python_wrapper(FileWriter& wrapperFile, Str className) const;
friend std::ostream& operator<<(std::ostream& os, const StaticMethod& m) {
for (size_t i = 0; i < m.nrOverloads(); i++)
os << "static " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
return os;
}
protected:
virtual void proxy_header(FileWriter& proxyFile) const;
std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
Str matlabUniqueName, int overload, int id,
const TypeAttributesTable& typeAttributes, const Qualified& instName =
Qualified()) const; ///< cpp wrapper
virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
Str matlabUniqueName, const ArgumentList& args,
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
const Qualified& instName) const;
};
} // \namespace wrap

71
wrap/TemplateInstantiationTypedef.cpp
View File

@ -19,43 +19,50 @@
#include "TemplateInstantiationTypedef.h"
#include "utilities.h"
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
using namespace std;
namespace wrap {
Class TemplateInstantiationTypedef::findAndExpand(const vector<Class>& classes) const {
// Find matching class
std::vector<Class>::const_iterator clsIt = classes.end();
for(std::vector<Class>::const_iterator it = classes.begin(); it != classes.end(); ++it) {
if(it->name == className && it->namespaces == classNamespaces && it->templateArgs.size() == typeList.size()) {
clsIt = it;
break;
}
Class TemplateInstantiationTypedef::findAndExpand(
const vector<Class>& classes) const {
// Find matching class
boost::optional<Class const &> matchedClass;
BOOST_FOREACH(const Class& cls, classes) {
if (cls.name == class_.name && cls.namespaces == class_.namespaces
&& cls.templateArgs.size() == typeList.size()) {
matchedClass.reset(cls);
break;
}
if(clsIt == classes.end())
throw DependencyMissing(wrap::qualifiedName("::", classNamespaces, className),
"instantiation into typedef name " + wrap::qualifiedName("::", namespaces, name) +
". Ensure that the typedef provides the correct number of template arguments.");
// Instantiate it
Class classInst = *clsIt;
for(size_t i = 0; i < typeList.size(); ++i)
classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i], namespaces, name);
// Fix class properties
classInst.name = name;
classInst.templateArgs.clear();
classInst.typedefName = clsIt->qualifiedName("::") + "<";
if(typeList.size() > 0)
classInst.typedefName += wrap::qualifiedName("::", typeList[0]);
for(size_t i = 1; i < typeList.size(); ++i)
classInst.typedefName += (", " + wrap::qualifiedName("::", typeList[i]));
classInst.typedefName += ">";
classInst.namespaces = namespaces;
return classInst;
}
}
if (!matchedClass)
throw DependencyMissing(class_.qualifiedName("::"),
"instantiation into typedef name " + qualifiedName("::")
+ ". Ensure that the typedef provides the correct number of template arguments.");
// Instantiate it
Class classInst = *matchedClass;
for (size_t i = 0; i < typeList.size(); ++i) {
TemplateSubstitution ts(classInst.templateArgs[i], typeList[i], *this);
classInst = classInst.expandTemplate(ts);
}
// Fix class properties
classInst.name = name;
classInst.templateArgs.clear();
classInst.typedefName = matchedClass->qualifiedName("::") + "<";
if (typeList.size() > 0)
classInst.typedefName += typeList[0].qualifiedName("::");
for (size_t i = 1; i < typeList.size(); ++i)
classInst.typedefName += (", " + typeList[i].qualifiedName("::"));
classInst.typedefName += ">";
classInst.namespaces = namespaces;
return classInst;
}
}

11
wrap/TemplateInstantiationTypedef.h
View File

@ -26,14 +26,11 @@
namespace wrap {
struct TemplateInstantiationTypedef {
std::vector<std::string> classNamespaces;
std::string className;
std::vector<std::string> namespaces;
std::string name;
std::vector<std::vector<std::string> > typeList;
struct TemplateInstantiationTypedef : public Qualified {
Qualified class_;
std::vector<Qualified> typeList;
Class findAndExpand(const std::vector<Class>& classes) const;
};
}
}

76
wrap/TemplateSubstitution.h Normal file
View File

@ -0,0 +1,76 @@
/* ----------------------------------------------------------------------------
* 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 TemplateSubstitution.h
* @brief Auxiliary class for template substitutions
* @author Frank Dellaert
* @date Nov 13, 2014
**/
#pragma once
#include "ReturnType.h"
#include <string>
#include <iostream>
namespace wrap {
/**
* e.g. TemplateSubstitution("T", gtsam::Point2, gtsam::PriorFactorPoint2)
*/
class TemplateSubstitution {
std::string templateArg_;
Qualified qualifiedType_, expandedClass_;
public:
TemplateSubstitution(const std::string& a, const Qualified& t,
const Qualified& e) :
templateArg_(a), qualifiedType_(t), expandedClass_(e) {
}
std::string expandedClassName() const {
return expandedClass_.name;
}
// Substitute if needed
Qualified operator()(const Qualified& type) const {
if (type.name == templateArg_ && type.namespaces.empty())
return qualifiedType_;
else if (type.name == "This")
return expandedClass_;
else
return type;
}
// Substitute if needed
ReturnType operator()(const ReturnType& type) const {
ReturnType instType = type;
if (type.name == templateArg_ && type.namespaces.empty())
instType.rename(qualifiedType_);
else if (type.name == "This")
instType.rename(expandedClass_);
return instType;
}
friend std::ostream& operator<<(std::ostream& os,
const TemplateSubstitution& ts) {
os << ts.templateArg_ << '/' << ts.qualifiedType_.qualifiedName("::")
<< " (" << ts.expandedClass_.qualifiedName("::") << ")";
return os;
}
};
} // \namespace wrap

16
wrap/TypeAttributesTable.cpp
View File

@ -46,13 +46,17 @@ namespace wrap {
void TypeAttributesTable::checkValidity(const vector<Class>& classes) const {
BOOST_FOREACH(const Class& cls, classes) {
// Check that class is virtual if it has a parent
if(!cls.qualifiedParent.empty() && !cls.isVirtual)
throw AttributeError(cls.qualifiedName("::"), "Has a base class so needs to be declared virtual, change to 'virtual class "+cls.name+" ...'");
if (!cls.qualifiedParent.empty() && !cls.isVirtual)
throw AttributeError(cls.qualifiedName("::"),
"Has a base class so needs to be declared virtual, change to 'virtual class "
+ cls.name + " ...'");
// Check that parent is virtual as well
if(!cls.qualifiedParent.empty() && !at(wrap::qualifiedName("::", cls.qualifiedParent)).isVirtual)
throw AttributeError(wrap::qualifiedName("::", cls.qualifiedParent),
"Is the base class of " + cls.qualifiedName("::") + ", so needs to be declared virtual");
Qualified parent = cls.qualifiedParent;
if (!parent.empty() && !at(parent.qualifiedName("::")).isVirtual)
throw AttributeError(parent.qualifiedName("::"),
"Is the base class of " + cls.qualifiedName("::")
+ ", so needs to be declared virtual");
}
}
}
}

4
wrap/TypeAttributesTable.h
View File

@ -28,7 +28,7 @@
namespace wrap {
// Forward declarations
struct Class;
class Class;
/** Attributes about valid classes, both for classes defined in this module and
* also those forward-declared from others. At the moment this only contains
@ -52,4 +52,4 @@ public:
void checkValidity(const std::vector<Class>& classes) const;
};
}
}

83
wrap/tests/expected-python/geometry_python.cpp Normal file
View File

@ -0,0 +1,83 @@
#include <boost/python.hpp>
using namespace boost::python;
BOOST_PYTHON_MODULE(geometry)
{
class_<Point2>("Point2")
.def("Point2", &Point2::Point2);
.def("argChar", &Point2::argChar);
.def("argUChar", &Point2::argUChar);
.def("dim", &Point2::dim);
.def("returnChar", &Point2::returnChar);
.def("vectorConfusion", &Point2::vectorConfusion);
.def("x", &Point2::x);
.def("y", &Point2::y);
;
class_<Point3>("Point3")
.def("Point3", &Point3::Point3);
.def("StaticFunctionRet", &Point3::StaticFunctionRet);
.def("staticFunction", &Point3::staticFunction);
.def("norm", &Point3::norm);
;
class_<Test>("Test")
.def("Test", &Test::Test);
.def("arg_EigenConstRef", &Test::arg_EigenConstRef);
.def("create_MixedPtrs", &Test::create_MixedPtrs);
.def("create_ptrs", &Test::create_ptrs);
.def("print", &Test::print);
.def("return_Point2Ptr", &Test::return_Point2Ptr);
.def("return_Test", &Test::return_Test);
.def("return_TestPtr", &Test::return_TestPtr);
.def("return_bool", &Test::return_bool);
.def("return_double", &Test::return_double);
.def("return_field", &Test::return_field);
.def("return_int", &Test::return_int);
.def("return_matrix1", &Test::return_matrix1);
.def("return_matrix2", &Test::return_matrix2);
.def("return_pair", &Test::return_pair);
.def("return_ptrs", &Test::return_ptrs);
.def("return_size_t", &Test::return_size_t);
.def("return_string", &Test::return_string);
.def("return_vector1", &Test::return_vector1);
.def("return_vector2", &Test::return_vector2);
;
class_<MyBase>("MyBase")
.def("MyBase", &MyBase::MyBase);
;
class_<MyTemplatePoint2>("MyTemplatePoint2")
.def("MyTemplatePoint2", &MyTemplatePoint2::MyTemplatePoint2);
.def("accept_T", &MyTemplatePoint2::accept_T);
.def("accept_Tptr", &MyTemplatePoint2::accept_Tptr);
.def("create_MixedPtrs", &MyTemplatePoint2::create_MixedPtrs);
.def("create_ptrs", &MyTemplatePoint2::create_ptrs);
.def("return_T", &MyTemplatePoint2::return_T);
.def("return_Tptr", &MyTemplatePoint2::return_Tptr);
.def("return_ptrs", &MyTemplatePoint2::return_ptrs);
.def("templatedMethod", &MyTemplatePoint2::templatedMethod);
.def("templatedMethod", &MyTemplatePoint2::templatedMethod);
;
class_<MyTemplatePoint3>("MyTemplatePoint3")
.def("MyTemplatePoint3", &MyTemplatePoint3::MyTemplatePoint3);
.def("accept_T", &MyTemplatePoint3::accept_T);
.def("accept_Tptr", &MyTemplatePoint3::accept_Tptr);
.def("create_MixedPtrs", &MyTemplatePoint3::create_MixedPtrs);
.def("create_ptrs", &MyTemplatePoint3::create_ptrs);
.def("return_T", &MyTemplatePoint3::return_T);
.def("return_Tptr", &MyTemplatePoint3::return_Tptr);
.def("return_ptrs", &MyTemplatePoint3::return_ptrs);
.def("templatedMethod", &MyTemplatePoint3::templatedMethod);
.def("templatedMethod", &MyTemplatePoint3::templatedMethod);
;
class_<MyFactorPosePoint2>("MyFactorPosePoint2")
.def("MyFactorPosePoint2", &MyFactorPosePoint2::MyFactorPosePoint2);
;
def("aGlobalFunction", aGlobalFunction);
def("overloadedGlobalFunction", overloadedGlobalFunction);
}

90
wrap/tests/expected2/+gtsam/Point2.m Normal file
View File

@ -0,0 +1,90 @@
%class Point2, see Doxygen page for details
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
%-------Constructors-------
%Point2()
%Point2(double x, double y)
%
%-------Methods-------
%argChar(char a) : returns void
%argUChar(unsigned char a) : returns void
%dim() : returns int
%returnChar() : returns char
%vectorConfusion() : returns VectorNotEigen
%x() : returns double
%y() : returns double
%
classdef Point2 < handle
properties
ptr_gtsamPoint2 = 0
end
methods
function obj = Point2(varargin)
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
my_ptr = varargin{2};
geometry_wrapper(0, my_ptr);
elseif nargin == 0
my_ptr = geometry_wrapper(1);
elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
my_ptr = geometry_wrapper(2, varargin{1}, varargin{2});
else
error('Arguments do not match any overload of gtsam.Point2 constructor');
end
obj.ptr_gtsamPoint2 = my_ptr;
end
function delete(obj)
geometry_wrapper(3, obj.ptr_gtsamPoint2);
end
function display(obj), obj.print(''); end
%DISPLAY Calls print on the object
function disp(obj), obj.display; end
%DISP Calls print on the object
function varargout = argChar(this, varargin)
% ARGCHAR usage: argChar(char a) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
geometry_wrapper(4, this, varargin{:});
end
function varargout = argUChar(this, varargin)
% ARGUCHAR usage: argUChar(unsigned char a) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
geometry_wrapper(5, this, varargin{:});
end
function varargout = dim(this, varargin)
% DIM usage: dim() : returns int
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
varargout{1} = geometry_wrapper(6, this, varargin{:});
end
function varargout = returnChar(this, varargin)
% RETURNCHAR usage: returnChar() : returns char
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
varargout{1} = geometry_wrapper(7, this, varargin{:});
end
function varargout = vectorConfusion(this, varargin)
% VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
varargout{1} = geometry_wrapper(8, this, varargin{:});
end
function varargout = x(this, varargin)
% X usage: x() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
varargout{1} = geometry_wrapper(9, this, varargin{:});
end
function varargout = y(this, varargin)
% Y usage: y() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
varargout{1} = geometry_wrapper(10, this, varargin{:});
end
end
methods(Static = true)
end
end

61
wrap/tests/expected2/+gtsam/Point3.m Normal file
View File

@ -0,0 +1,61 @@
%class Point3, see Doxygen page for details
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
%-------Constructors-------
%Point3(double x, double y, double z)
%
%-------Methods-------
%norm() : returns double
%
%-------Static Methods-------
%StaticFunctionRet(double z) : returns gtsam::Point3
%staticFunction() : returns double
%
classdef Point3 < handle
properties
ptr_gtsamPoint3 = 0
end
methods
function obj = Point3(varargin)
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
my_ptr = varargin{2};
geometry_wrapper(11, my_ptr);
elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')
my_ptr = geometry_wrapper(12, varargin{1}, varargin{2}, varargin{3});
else
error('Arguments do not match any overload of gtsam.Point3 constructor');
end
obj.ptr_gtsamPoint3 = my_ptr;
end
function delete(obj)
geometry_wrapper(13, obj.ptr_gtsamPoint3);
end
function display(obj), obj.print(''); end
%DISPLAY Calls print on the object
function disp(obj), obj.display; end
%DISP Calls print on the object
function varargout = norm(this, varargin)
% NORM usage: norm() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
varargout{1} = geometry_wrapper(14, this, varargin{:});
end
end
methods(Static = true)
function varargout = StaticFunctionRet(varargin)
% STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
varargout{1} = geometry_wrapper(15, varargin{:});
end
function varargout = StaticFunction(varargin)
% STATICFUNCTION usage: staticFunction() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
varargout{1} = geometry_wrapper(16, varargin{:});
end
end
end

35
wrap/tests/expected2/MyBase.m Normal file
View File

@ -0,0 +1,35 @@
%class MyBase, see Doxygen page for details
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
classdef MyBase < handle
properties
ptr_MyBase = 0
end
methods
function obj = MyBase(varargin)
if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
if nargin == 2
my_ptr = varargin{2};
else
my_ptr = geometry_wrapper(41, varargin{2});
end
geometry_wrapper(40, my_ptr);
else
error('Arguments do not match any overload of MyBase constructor');
end
obj.ptr_MyBase = my_ptr;
end
function delete(obj)
geometry_wrapper(42, obj.ptr_MyBase);
end
function display(obj), obj.print(''); end
%DISPLAY Calls print on the object
function disp(obj), obj.display; end
%DISP Calls print on the object
end
methods(Static = true)
end
end

36
wrap/tests/expected2/MyFactorPosePoint2.m Normal file
View File

@ -0,0 +1,36 @@
%class MyFactorPosePoint2, see Doxygen page for details
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
%-------Constructors-------
%MyFactorPosePoint2(size_t key1, size_t key2, double measured, Base noiseModel)
%
classdef MyFactorPosePoint2 < handle
properties
ptr_MyFactorPosePoint2 = 0
end
methods
function obj = MyFactorPosePoint2(varargin)
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
my_ptr = varargin{2};
geometry_wrapper(69, my_ptr);
elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base')
my_ptr = geometry_wrapper(70, varargin{1}, varargin{2}, varargin{3}, varargin{4});
else
error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
end
obj.ptr_MyFactorPosePoint2 = my_ptr;
end
function delete(obj)
geometry_wrapper(71, obj.ptr_MyFactorPosePoint2);
end
function display(obj), obj.print(''); end
%DISPLAY Calls print on the object
function disp(obj), obj.display; end
%DISP Calls print on the object
end
methods(Static = true)
end
end

134
wrap/tests/expected2/MyTemplatePoint2.m Normal file
View File

@ -0,0 +1,134 @@
%class MyTemplatePoint2, see Doxygen page for details
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
%-------Constructors-------
%MyTemplatePoint2()
%
%-------Methods-------
%accept_T(Point2 value) : returns void
%accept_Tptr(Point2 value) : returns void
%create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
%create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
%return_T(Point2 value) : returns gtsam::Point2
%return_Tptr(Point2 value) : returns gtsam::Point2
%return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
%templatedMethodPoint2(Point2 t) : returns void
%templatedMethodPoint3(Point3 t) : returns void
%
classdef MyTemplatePoint2 < MyBase
properties
ptr_MyTemplatePoint2 = 0
end
methods
function obj = MyTemplatePoint2(varargin)
if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
if nargin == 2
my_ptr = varargin{2};
else
my_ptr = geometry_wrapper(44, varargin{2});
end
base_ptr = geometry_wrapper(43, my_ptr);
elseif nargin == 0
[ my_ptr, base_ptr ] = geometry_wrapper(45);
else
error('Arguments do not match any overload of MyTemplatePoint2 constructor');
end
obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
obj.ptr_MyTemplatePoint2 = my_ptr;
end
function delete(obj)
geometry_wrapper(46, obj.ptr_MyTemplatePoint2);
end
function display(obj), obj.print(''); end
%DISPLAY Calls print on the object
function disp(obj), obj.display; end
%DISP Calls print on the object
function varargout = accept_T(this, varargin)
% ACCEPT_T usage: accept_T(Point2 value) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
geometry_wrapper(47, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint2.accept_T');
end
end
function varargout = accept_Tptr(this, varargin)
% ACCEPT_TPTR usage: accept_Tptr(Point2 value) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
geometry_wrapper(48, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr');
end
end
function varargout = create_MixedPtrs(this, varargin)
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
[ varargout{1} varargout{2} ] = geometry_wrapper(49, this, varargin{:});
end
function varargout = create_ptrs(this, varargin)
% CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
[ varargout{1} varargout{2} ] = geometry_wrapper(50, this, varargin{:});
end
function varargout = return_T(this, varargin)
% RETURN_T usage: return_T(Point2 value) : returns gtsam::Point2
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
varargout{1} = geometry_wrapper(51, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint2.return_T');
end
end
function varargout = return_Tptr(this, varargin)
% RETURN_TPTR usage: return_Tptr(Point2 value) : returns gtsam::Point2
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
varargout{1} = geometry_wrapper(52, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr');
end
end
function varargout = return_ptrs(this, varargin)
% RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 2 && isa(varargin{1},'gtsam.Point2') && isa(varargin{2},'gtsam.Point2')
[ varargout{1} varargout{2} ] = geometry_wrapper(53, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs');
end
end
function varargout = templatedMethodPoint2(this, varargin)
% TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
geometry_wrapper(54, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
end
end
function varargout = templatedMethodPoint3(this, varargin)
% TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
geometry_wrapper(55, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
end
end
end
methods(Static = true)
end
end

134
wrap/tests/expected2/MyTemplatePoint3.m Normal file
View File

@ -0,0 +1,134 @@
%class MyTemplatePoint3, see Doxygen page for details
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
%-------Constructors-------
%MyTemplatePoint3()
%
%-------Methods-------
%accept_T(Point3 value) : returns void
%accept_Tptr(Point3 value) : returns void
%create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::Point3 >
%create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 >
%return_T(Point3 value) : returns gtsam::Point3
%return_Tptr(Point3 value) : returns gtsam::Point3
%return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 >
%templatedMethodPoint2(Point2 t) : returns void
%templatedMethodPoint3(Point3 t) : returns void
%
classdef MyTemplatePoint3 < MyBase
properties
ptr_MyTemplatePoint3 = 0
end
methods
function obj = MyTemplatePoint3(varargin)
if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
if nargin == 2
my_ptr = varargin{2};
else
my_ptr = geometry_wrapper(57, varargin{2});
end
base_ptr = geometry_wrapper(56, my_ptr);
elseif nargin == 0
[ my_ptr, base_ptr ] = geometry_wrapper(58);
else
error('Arguments do not match any overload of MyTemplatePoint3 constructor');
end
obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
obj.ptr_MyTemplatePoint3 = my_ptr;
end
function delete(obj)
geometry_wrapper(59, obj.ptr_MyTemplatePoint3);
end
function display(obj), obj.print(''); end
%DISPLAY Calls print on the object
function disp(obj), obj.display; end
%DISP Calls print on the object
function varargout = accept_T(this, varargin)
% ACCEPT_T usage: accept_T(Point3 value) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
geometry_wrapper(60, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint3.accept_T');
end
end
function varargout = accept_Tptr(this, varargin)
% ACCEPT_TPTR usage: accept_Tptr(Point3 value) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
geometry_wrapper(61, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint3.accept_Tptr');
end
end
function varargout = create_MixedPtrs(this, varargin)
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::Point3 >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
[ varargout{1} varargout{2} ] = geometry_wrapper(62, this, varargin{:});
end
function varargout = create_ptrs(this, varargin)
% CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
[ varargout{1} varargout{2} ] = geometry_wrapper(63, this, varargin{:});
end
function varargout = return_T(this, varargin)
% RETURN_T usage: return_T(Point3 value) : returns gtsam::Point3
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
varargout{1} = geometry_wrapper(64, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint3.return_T');
end
end
function varargout = return_Tptr(this, varargin)
% RETURN_TPTR usage: return_Tptr(Point3 value) : returns gtsam::Point3
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
varargout{1} = geometry_wrapper(65, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint3.return_Tptr');
end
end
function varargout = return_ptrs(this, varargin)
% RETURN_PTRS usage: return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 2 && isa(varargin{1},'gtsam.Point3') && isa(varargin{2},'gtsam.Point3')
[ varargout{1} varargout{2} ] = geometry_wrapper(66, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint3.return_ptrs');
end
end
function varargout = templatedMethodPoint2(this, varargin)
% TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
geometry_wrapper(67, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
end
end
function varargout = templatedMethodPoint3(this, varargin)
% TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
geometry_wrapper(68, this, varargin{:});
else
error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
end
end
end
methods(Static = true)
end
end

Some files were not shown because too many files have changed in this diff Show More