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> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -3222,22 +3254,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>

4
CMakeLists.txt
View File

@ -9,8 +9,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
endif() endif()
# Set the version number for the library # Set the version number for the library
set (GTSAM_VERSION_MAJOR 3) set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 1) set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 0) set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") 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}") 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; size_t dim() const;
}; };
class Vector3 {
Vector3(Vector v);
};
class Vector6 {
Vector6(Vector v);
};
#include <gtsam/base/LieScalar.h> #include <gtsam/base/LieScalar.h>
virtual class LieScalar : gtsam::Value { class LieScalar {
// Standard constructors // Standard constructors
LieScalar(); LieScalar();
LieScalar(double d); LieScalar(double d);
@ -186,7 +192,7 @@ virtual class LieScalar : gtsam::Value {
}; };
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
virtual class LieVector : gtsam::Value { class LieVector {
// Standard constructors // Standard constructors
LieVector(); LieVector();
LieVector(Vector v); LieVector(Vector v);
@ -218,7 +224,7 @@ virtual class LieVector : gtsam::Value {
}; };
#include <gtsam/base/LieMatrix.h> #include <gtsam/base/LieMatrix.h>
virtual class LieMatrix : gtsam::Value { class LieMatrix {
// Standard constructors // Standard constructors
LieMatrix(); LieMatrix();
LieMatrix(Matrix v); LieMatrix(Matrix v);
@ -253,7 +259,7 @@ virtual class LieMatrix : gtsam::Value {
// geometry // geometry
//************************************************************************* //*************************************************************************
virtual class Point2 : gtsam::Value { class Point2 {
// Standard Constructors // Standard Constructors
Point2(); Point2();
Point2(double x, double y); Point2(double x, double y);
@ -290,7 +296,7 @@ virtual class Point2 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class StereoPoint2 : gtsam::Value { class StereoPoint2 {
// Standard Constructors // Standard Constructors
StereoPoint2(); StereoPoint2();
StereoPoint2(double uL, double uR, double v); StereoPoint2(double uL, double uR, double v);
@ -325,7 +331,7 @@ virtual class StereoPoint2 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Point3 : gtsam::Value { class Point3 {
// Standard Constructors // Standard Constructors
Point3(); Point3();
Point3(double x, double y, double z); Point3(double x, double y, double z);
@ -361,7 +367,7 @@ virtual class Point3 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Rot2 : gtsam::Value { class Rot2 {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
Rot2(); Rot2();
Rot2(double theta); Rot2(double theta);
@ -406,7 +412,7 @@ virtual class Rot2 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Rot3 : gtsam::Value { class Rot3 {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
Rot3(); Rot3();
Rot3(Matrix R); Rot3(Matrix R);
@ -462,7 +468,7 @@ virtual class Rot3 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Pose2 : gtsam::Value { class Pose2 {
// Standard Constructor // Standard Constructor
Pose2(); Pose2();
Pose2(const gtsam::Pose2& pose); Pose2(const gtsam::Pose2& pose);
@ -512,7 +518,7 @@ virtual class Pose2 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Pose3 : gtsam::Value { class Pose3 {
// Standard Constructors // Standard Constructors
Pose3(); Pose3();
Pose3(const gtsam::Pose3& pose); Pose3(const gtsam::Pose3& pose);
@ -564,7 +570,7 @@ virtual class Pose3 : gtsam::Value {
}; };
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
virtual class Unit3 : gtsam::Value { class Unit3 {
// Standard Constructors // Standard Constructors
Unit3(); Unit3();
Unit3(const gtsam::Point3& pose); Unit3(const gtsam::Point3& pose);
@ -585,7 +591,7 @@ virtual class Unit3 : gtsam::Value {
}; };
#include <gtsam/geometry/EssentialMatrix.h> #include <gtsam/geometry/EssentialMatrix.h>
virtual class EssentialMatrix : gtsam::Value { class EssentialMatrix {
// Standard Constructors // Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
@ -606,7 +612,7 @@ virtual class EssentialMatrix : gtsam::Value {
double error(Vector vA, Vector vB); double error(Vector vA, Vector vB);
}; };
virtual class Cal3_S2 : gtsam::Value { class Cal3_S2 {
// Standard Constructors // Standard Constructors
Cal3_S2(); Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0); 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> #include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Value { class Cal3DS2 {
// Standard Constructors // Standard Constructors
Cal3DS2(); Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); 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; 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 // Standard Constructors and Named Constructors
CalibratedCamera(); CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose); CalibratedCamera(const gtsam::Pose3& pose);
@ -732,7 +774,7 @@ virtual class CalibratedCamera : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class SimpleCamera : gtsam::Value { class SimpleCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
SimpleCamera(); SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose); SimpleCamera(const gtsam::Pose3& pose);
@ -771,7 +813,7 @@ virtual class SimpleCamera : gtsam::Value {
}; };
template<CALIBRATION = {gtsam::Cal3DS2}> template<CALIBRATION = {gtsam::Cal3DS2}>
virtual class PinholeCamera : gtsam::Value { class PinholeCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
PinholeCamera(); PinholeCamera();
PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose);
@ -809,7 +851,7 @@ virtual class PinholeCamera : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class StereoCamera : gtsam::Value { class StereoCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
StereoCamera(); StereoCamera();
StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
@ -862,7 +904,7 @@ virtual class SymbolicFactor {
}; };
#include <gtsam/symbolic/SymbolicFactorGraph.h> #include <gtsam/symbolic/SymbolicFactorGraph.h>
class SymbolicFactorGraph { virtual class SymbolicFactorGraph {
SymbolicFactorGraph(); SymbolicFactorGraph();
SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
@ -1664,15 +1706,12 @@ class Values {
void print(string s) const; void print(string s) const;
bool equals(const gtsam::Values& other, double tol) 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 insert(const gtsam::Values& values);
void update(size_t j, const gtsam::Value& val);
void update(const gtsam::Values& values); void update(const gtsam::Values& values);
void erase(size_t j); void erase(size_t j);
void swap(gtsam::Values& values); void swap(gtsam::Values& values);
bool exists(size_t j) const; bool exists(size_t j) const;
gtsam::Value at(size_t j) const;
gtsam::KeyList keys() const; gtsam::KeyList keys() const;
gtsam::VectorValues zeroVectors() const; gtsam::VectorValues zeroVectors() const;
@ -1682,6 +1721,37 @@ class Values {
// enabling serialization functionality // enabling serialization functionality
void serialize() const; 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> // Actually a FastList<Key>
@ -2077,7 +2147,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.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 { virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const; T prior() const;
@ -2088,7 +2158,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/BetweenFactor.h> #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 { virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const; T measured() const;
@ -2099,7 +2169,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h> #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 { virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation // Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible); NonlinearEquality(size_t j, const T& feasible);
@ -2280,7 +2350,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph,
namespace imuBias { namespace imuBias {
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
virtual class ConstantBias : gtsam::Value { class ConstantBias {
// Standard Constructor // Standard Constructor
ConstantBias(); ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro); ConstantBias(Vector biasAcc, Vector biasGyro);
@ -2340,7 +2410,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; 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::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const; Vector gravity, Vector omegaCoriolis) const;
@ -2383,7 +2453,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; 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::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const; 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/LU>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry> #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 // equals for Matrix types
template<int M, int N, int Options> template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct equals<Eigen::Matrix<double, M, N, Options> > { struct equals<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
typedef Eigen::Matrix<double, M, N, Options> type; typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> type;
typedef bool result_type; typedef bool result_type;
bool operator()(const type& A, const type& B, double tol) { bool operator()(const type& A, const type& B, double tol) {
return equal_with_abs_tol(A, B, 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 // print for Matrix types
template<int M, int N, int Options> template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct print<Eigen::Matrix<double, M, N, Options> > { struct print<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
typedef Eigen::Matrix<double, M, N, Options> type; typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> type;
typedef void result_type; typedef void result_type;
void operator()(const type& A, const std::string& str) { void operator()(const type& A, const std::string& str) {
std::cout << str << ": " << A << std::endl; 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), 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"); "DefaultChart has not been implemented yet for dynamically sized matrices");
static vector local(const T& origin, const T& other) { 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) { 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) { static int getDimension(const T&origin) {
return origin.rows() * origin.cols(); 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(); } inline Matrix trans(const Matrix& A) { return A.transpose(); }
/// Reshape functor /// 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 { 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) //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) { static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
return in.data(); 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) /// 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> 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; typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) { static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
return in; return in;
@ -314,7 +314,7 @@ struct Reshape<M, M, M, M, InOptions> {
/// Reshape specialization that does nothing as shape stays the same /// Reshape specialization that does nothing as shape stays the same
template <int M, int N, int InOptions> 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; typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) { static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
return in; return in;
@ -323,17 +323,17 @@ struct Reshape<M, N, M, N, InOptions> {
/// Reshape specialization that does transpose /// Reshape specialization that does transpose
template <int M, int N, int InOptions> 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; typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) { static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
return in.transpose(); return in.transpose();
} }
}; };
template <int OutM, int OutN, int InM, int InN, int InOptions> template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
inline typename Reshape<OutM, OutN, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){ 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); 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 ] * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> { class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
typedef Cal3DS2_Base Base; typedef Cal3DS2_Base Base;
@ -87,21 +87,24 @@ public:
/// Return dimensions of calibration manifold object /// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable static size_t Dim() { return 9; } //TODO: make a final dimension variable
/// @}
private: private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) 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", ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Cal3DS2_Base>(*this)); boost::serialization::base_object<Cal3DS2_Base>(*this));
} }
/// @}
}; };
// Define GTSAM traits // 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, double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) { const Matrix2& DK) {
Eigen::Matrix<double, 2, 5> DR1; Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; 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, // DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy; y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D; Matrix29 D;
D << DR1, DK * DR2; D << DR1, DK * DR2;
return D; 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, 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 drdx = 2. * x;
const double drdy = 2. * y; const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; 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 dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); 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, // DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy; 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, Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix&> H2) const { boost::optional<Matrix29&> H1,
boost::optional<Matrix2&> H2) const {
// rr = x^2 + y^2; // rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^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 pnx = g * x + dx;
const double pny = g * y + dy; const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_; if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration // 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_); 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 { Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion. // 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 rr = xx + yy;
const double r4 = rr * rr; const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4); const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); 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 dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx; const double pnx = g * x + dx;
const double pny = g * y + dy; const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); 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 #pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
namespace gtsam { namespace gtsam {
@ -114,9 +113,14 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates * @return point in (distorted) image coordinates
*/ */
Point2 uncalibrate(const Point2& p, Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none, boost::optional<Matrix29&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ; 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 /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const; Point2 calibrate(const Point2& p, const double tol=1e-5) const;
@ -127,7 +131,6 @@ public:
/// Derivative of uncalibrate wrpt the calibration parameters /// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ; Matrix D2d_calibration(const Point2& p) const ;
private: 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, Point2 Cal3Unified::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
@ -70,26 +71,30 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
// Part2: project NPlane point to pixel plane: use Cal3DS2 // Part2: project NPlane point to pixel plane: use Cal3DS2
Point2 m(x,y); 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); Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
// Inlined derivative for calibration // Inlined derivative for calibration
if (H1) { if (H1) {
// part1 // part1
Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, Vector2 DU;
-ys * sqrt_nx * xi_sqrt_nx2); DU << -xs * sqrt_nx * xi_sqrt_nx2, //
Matrix DDS2U = H2base * DU; -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 // Inlined derivative for points
if (H2) { if (H2) {
// part1 // part1
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
const double mid = -(xi * xs*ys) * denom; const double mid = -(xi * xs*ys) * denom;
Matrix DU = (Matrix(2, 2) << Matrix2 DU;
(sqrt_nx + xi*(ys*ys + 1)) * denom, mid, DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
*H2 = H2base * DU; *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 ] * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> { class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
typedef Cal3Unified This; typedef Cal3Unified This;
typedef Cal3DS2_Base Base; typedef Cal3DS2_Base Base;
@ -129,8 +129,6 @@ private:
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) 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", ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this)); boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_); 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)); return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
} }
virtual ~EssentialMatrix() {}
/// @} /// @}
/// @name Testable /// @name Testable

2
gtsam/linear/JacobianFactor.h
View File

@ -187,7 +187,7 @@ namespace gtsam {
/// Return the diagonal of the Hessian for this factor /// Return the diagonal of the Hessian for this factor
virtual VectorValues hessianDiagonal() const; virtual VectorValues hessianDiagonal() const;
/* ************************************************************************* */ /// Raw memory access version of hessianDiagonal
virtual void hessianDiagonal(double* d) const; virtual void hessianDiagonal(double* d) const;
/// Return the block diagonal of the Hessian for this factor /// 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 <gtsam/linear/NoiseModel.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/map.hpp>
#include <iostream> #include <iostream>
#include <vector> #include <vector>
@ -134,30 +135,16 @@ void BlockJacobiPreconditioner::build(
size_t nnz = 0; size_t nnz = 0;
for ( size_t i = 0 ; i < n ; ++i ) { for ( size_t i = 0 ; i < n ; ++i ) {
const size_t dim = dims_[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+1)) >> 1); // d*(d+1) / 2 ;
nnz += dim*dim; 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 ) { BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) { std::map<Key, Matrix> hessianMap = gf->hessianBlockDiagonal();
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
const KeyInfoEntry &entry = keyInfo.find(*it)->second; blocks.push_back(hessian);
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.");
}
} }
/* if necessary, allocating the memory for cacheing the factorization results */ /* 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 { class PreintegratedMeasurements {
public: public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration 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 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) Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
@ -216,11 +216,21 @@ namespace gtsam {
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_pos, H_vel_vel, H_vel_angles,
H_angles_pos, H_angles_vel, H_angles_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 // 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 ; 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 // Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){ if(!use2ndOrderIntegration_){

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

@ -215,7 +215,7 @@ namespace gtsam {
Values::Values(const Values::ConstFiltered<ValueType>& view) { Values::Values(const Values::ConstFiltered<ValueType>& view) {
BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) { BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) {
Key key = key_value.key; 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); expectedSubValues1.insert(3, pose3);
EXPECT(assert_equal(expectedSubValues1, actualSubValues1)); 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 // Filter by type
i = 0; i = 0;
Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>(); 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 * @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
* @author Frank Dellaert * @author Frank Dellaert
* @author Luca Carlone * @author Luca Carlone
@ -17,13 +17,13 @@
namespace gtsam { namespace gtsam {
/** /**
* ImplicitSchurFactor * RegularImplicitSchurFactor
*/ */
template<size_t D> // template<size_t D> //
class ImplicitSchurFactor: public GaussianFactor { class RegularImplicitSchurFactor: public GaussianFactor {
public: 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 typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected: protected:
@ -40,11 +40,11 @@ protected:
public: public:
/// Constructor /// Constructor
ImplicitSchurFactor() { RegularImplicitSchurFactor() {
} }
/// Construct from blcoks of F, E, inv(E'*E), and RHS vector b /// 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) : const Matrix3& P, const Vector& b) :
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
initKeys(); initKeys();
@ -58,7 +58,7 @@ public:
} }
/// Destructor /// Destructor
virtual ~ImplicitSchurFactor() { virtual ~RegularImplicitSchurFactor() {
} }
// Write access, only use for construction! // Write access, only use for construction!
@ -87,7 +87,7 @@ public:
/// print /// print
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << " ImplicitSchurFactor " << std::endl; std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s); Factor::print(s);
std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
std::cout << " E_ \n" << E_ << std::endl; std::cout << " E_ \n" << E_ << std::endl;
@ -96,7 +96,7 @@ public:
/// equals /// equals
bool equals(const GaussianFactor& lf, double tol) const { bool equals(const GaussianFactor& lf, double tol) const {
if (!dynamic_cast<const ImplicitSchurFactor*>(&lf)) if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
return false; return false;
else { else {
return false; return false;
@ -110,21 +110,21 @@ public:
virtual Matrix augmentedJacobian() const { virtual Matrix augmentedJacobian() const {
throw std::runtime_error( throw std::runtime_error(
"ImplicitSchurFactor::augmentedJacobian non implemented"); "RegularImplicitSchurFactor::augmentedJacobian non implemented");
return Matrix(); return Matrix();
} }
virtual std::pair<Matrix, Vector> jacobian() const { 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()); return std::make_pair(Matrix(), Vector());
} }
virtual Matrix augmentedInformation() const { virtual Matrix augmentedInformation() const {
throw std::runtime_error( throw std::runtime_error(
"ImplicitSchurFactor::augmentedInformation non implemented"); "RegularImplicitSchurFactor::augmentedInformation non implemented");
return Matrix(); return Matrix();
} }
virtual Matrix information() const { virtual Matrix information() const {
throw std::runtime_error( throw std::runtime_error(
"ImplicitSchurFactor::information non implemented"); "RegularImplicitSchurFactor::information non implemented");
return Matrix(); return Matrix();
} }
@ -210,18 +210,18 @@ public:
} }
virtual GaussianFactor::shared_ptr clone() const { virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_, return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_); PointCovariance_, E_, b_);
throw std::runtime_error("ImplicitSchurFactor::clone non implemented"); throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
} }
virtual bool empty() const { virtual bool empty() const {
return false; return false;
} }
virtual GaussianFactor::shared_ptr negate() const { virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_, return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_); 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 // 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 "JacobianFactorQ.h"
#include "JacobianFactorSVD.h" #include "JacobianFactorSVD.h"
#include "ImplicitSchurFactor.h" #include "RegularImplicitSchurFactor.h"
#include "RegularHessianFactor.h" #include "RegularHessianFactor.h"
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
@ -73,7 +73,7 @@ public:
/** /**
* Constructor * 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) : SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor) { body_P_sensor_(body_P_sensor) {
@ -271,8 +271,13 @@ public:
Vector bi; Vector bi;
try { try {
bi = bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
-(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&) { } catch (CheiralityException&) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit(EXIT_FAILURE); 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, const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
typename boost::shared_ptr<ImplicitSchurFactor<D> > f( typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
new ImplicitSchurFactor<D>()); new RegularImplicitSchurFactor<D>());
computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(),
cameras, point, lambda, diagonalDamping); cameras, point, lambda, diagonalDamping);
f->initKeys(); 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, * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected * otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations * @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, SmartProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI, const bool manageDegeneracy, const bool enableEPI,
@ -298,7 +298,7 @@ public:
|| (!this->manageDegeneracy_ || (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) { && (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) { if (isDebug) {
std::cout << "createImplicitSchurFactor: degenerate configuration" std::cout << "createRegularImplicitSchurFactor: degenerate configuration"
<< std::endl; << std::endl;
} }
return false; return false;
@ -409,12 +409,12 @@ public:
} }
// create factor // create factor
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor( boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createImplicitSchurFactor(cameras, point_, lambda); return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
else else
return boost::shared_ptr<ImplicitSchurFactor<D> >(); return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
} }
/// create factor /// create factor
@ -685,7 +685,7 @@ public:
inline bool isPointBehindCamera() const { inline bool isPointBehindCamera() const {
return cheiralityException_; return cheiralityException_;
} }
/** return chirality verbosity */ /** return cheirality verbosity */
inline bool verboseCheirality() const { inline bool verboseCheirality() const {
return verboseCheirality_; 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, * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected * otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations * @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, SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false, const double linThreshold = -1, const bool manageDegeneracy = false,
@ -157,6 +157,9 @@ public:
size_t i=0; size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) { BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k); 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++]); typename Base::Camera camera(pose, *K_all_[i++]);
cameras.push_back(camera); 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_unstable/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/ImplicitSchurFactor.h> #include <gtsam/slam/RegularImplicitSchurFactor.h>
//#include <gtsam_unstable/slam/JacobianFactorQ.h> //#include <gtsam_unstable/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQ.h> #include <gtsam/slam/JacobianFactorQ.h>
//#include "gtsam_unstable/slam/JacobianFactorQR.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.); 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 = Matrix::Ones(6,3);
Matrix E = zeros(6, 3); Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2); E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3); E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse(); Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> expected(Fblocks, E, P, b); RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
Matrix expectedP = expected.getPointCovariance(); Matrix expectedP = expected.getPointCovariance();
EXPECT(assert_equal(expectedP, P)); EXPECT(assert_equal(expectedP, P));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( implicitSchurFactor, addHessianMultiply ) { TEST( regularImplicitSchurFactor, addHessianMultiply ) {
Matrix E = zeros(6, 3); Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2); E.block<2,2>(0, 0) = eye(2);
@ -81,11 +81,11 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
keys += 0,1,2,3; keys += 0,1,2,3;
Vector x = xvalues.vector(keys); Vector x = xvalues.vector(keys);
Vector expected = zero(24); 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)); EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
// Create ImplicitSchurFactor // 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 VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
{ // First Version { // First Version
@ -190,7 +190,7 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(implicitSchurFactor, hessianDiagonal) TEST(regularImplicitSchurFactor, hessianDiagonal)
{ {
/* TESTED AGAINST MATLAB /* TESTED AGAINST MATLAB
* F = [ones(2,6) zeros(2,6) zeros(2,6) * 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>(2, 0) << 1,2,3,4,5,6;
E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
Matrix3 P = (E.transpose() * E).inverse(); Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> factor(Fblocks, E, P, b); RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
// hessianDiagonal // hessianDiagonal
VectorValues expected; VectorValues expected;

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

@ -292,6 +292,95 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
if(isDebugTest) tictoc_print_(); 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 ){ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
// cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; // 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 // specify the classes from gtsam we are using
virtual class gtsam::Value; virtual class gtsam::Value;
virtual class gtsam::LieScalar; class gtsam::Vector6;
virtual class gtsam::LieVector; class gtsam::LieScalar;
virtual class gtsam::Point2; class gtsam::LieVector;
virtual class gtsam::Rot2; class gtsam::Point2;
virtual class gtsam::Pose2; class gtsam::Rot2;
virtual class gtsam::Point3; class gtsam::Pose2;
virtual class gtsam::Rot3; class gtsam::Point3;
virtual class gtsam::Pose3; class gtsam::Rot3;
class gtsam::Pose3;
virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Base;
virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::noiseModel::Gaussian;
virtual class gtsam::imuBias::ConstantBias; virtual class gtsam::imuBias::ConstantBias;
@ -23,8 +24,8 @@ virtual class gtsam::NoiseModelFactor4;
virtual class gtsam::GaussianFactor; virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor; virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor; virtual class gtsam::JacobianFactor;
virtual class gtsam::Cal3_S2; class gtsam::Cal3_S2;
virtual class gtsam::Cal3DS2; class gtsam::Cal3DS2;
class gtsam::GaussianFactorGraph; class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph; class gtsam::NonlinearFactorGraph;
class gtsam::Ordering; class gtsam::Ordering;
@ -48,7 +49,7 @@ class Dummy {
}; };
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
virtual class PoseRTV : gtsam::Value { class PoseRTV {
PoseRTV(); PoseRTV();
PoseRTV(Vector rtv); PoseRTV(Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); 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> #include <gtsam_unstable/geometry/Pose3Upright.h>
virtual class Pose3Upright : gtsam::Value { class Pose3Upright {
Pose3Upright(); Pose3Upright();
Pose3Upright(const gtsam::Pose3Upright& x); Pose3Upright(const gtsam::Pose3Upright& x);
Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t); Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
@ -137,7 +138,7 @@ virtual class Pose3Upright : gtsam::Value {
}; // \class Pose3Upright }; // \class Pose3Upright
#include <gtsam_unstable/geometry/BearingS2.h> #include <gtsam_unstable/geometry/BearingS2.h>
virtual class BearingS2 : gtsam::Value { class BearingS2 {
BearingS2(); BearingS2();
BearingS2(double azimuth, double elevation); BearingS2(double azimuth, double elevation);
BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation); BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);
@ -520,14 +521,14 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
virtual class Reconstruction : gtsam::NonlinearFactor { virtual class Reconstruction : gtsam::NonlinearFactor {
Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); 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 { virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor {
DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey,
double h, Matrix Inertia, Vector Fu, double m); 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 /// Access via key
VerticalBlockMatrix::Block operator()(Key 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(); DenseIndex block = it - keys_.begin();
return Ab_(block); return Ab_(block);
} }
@ -98,7 +99,7 @@ struct CallRecord {
template<int ROWS, int COLS> template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA, void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) { 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) /// Handle Leaf Case for Dynamic Matrix type (slower)
template<> template<>
@ -311,9 +312,9 @@ public:
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/// Leaf Expression /// Leaf Expression
template<class T, class Chart=DefaultChart<T> > template<class T, class Chart = DefaultChart<T> >
class LeafExpression: public ExpressionNode<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 /// The key into values
Key key_; Key key_;
@ -347,8 +348,8 @@ public:
} }
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual const value_type& traceExecution(const Values& values, ExecutionTrace<value_type>& trace, virtual const value_type& traceExecution(const Values& values,
char* raw) const { ExecutionTrace<value_type>& trace, char* raw) const {
trace.setLeaf(key_); trace.setLeaf(key_);
return dynamic_cast<const value_type&>(values.at(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 /// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
template<typename T> template<typename T>
class LeafExpression<T, DefaultChart<T> >: public ExpressionNode<T> { class LeafExpression<T, DefaultChart<T> > : public ExpressionNode<T> {
typedef T value_type; typedef T value_type;
/// The key into values /// The key into values
@ -405,6 +406,7 @@ public:
// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost // C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. // and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education.
// to recursively generate a class, that will be the base for function nodes. // 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 // The class generated, for three arguments A1, A2, and A3 will be
// //
// struct Base1 : Argument<T,A1,1>, FunctionalBase<T> { // 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 // All this magic happens when we generate the Base3 base class of FunctionalNode
// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode // 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 /// meta-function to generate fixed-size JacobianTA type
@ -457,6 +483,7 @@ struct FunctionalBase: ExpressionNode<T> {
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record, char*& raw) const { 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> template<class T, class TYPES>
struct FunctionalNode { struct FunctionalNode {
/// The following typedef generates the recursively defined Base class
typedef typename boost::mpl::fold<TYPES, FunctionalBase<T>, typedef typename boost::mpl::fold<TYPES, FunctionalBase<T>,
GenerateFunctionalNode<T, MPL::_2, MPL::_1> >::type Base; 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 { struct type: public Base {
// Argument types and derived, note these are base 0 ! // 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 TYPES Arguments;
typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians; typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals; typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
#endif
/// Reset expression shared pointer /// Reset expression shared pointer
template<class A, size_t N> template<class A, size_t N>
@ -725,7 +760,8 @@ public:
typedef boost::function< typedef boost::function<
T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type, 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 FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record; typedef typename Base::Record Record;

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

@ -22,6 +22,7 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <algorithm>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -143,7 +144,7 @@ TEST(ExpressionFactor, Triple) {
// Test out invoke // Test out invoke
TEST(ExpressionFactor, 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?) // Creating a Pose3 (is there another way?)
boost::fusion::vector<Rot3, Point3> pair; 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/shared_array.hpp>
#include <boost/timer.hpp> #include <boost/timer.hpp>
#include "FindSeparator.h"
extern "C" { extern "C" {
#include <metis.h> #include <metis.h>
#include "metislib.h" #include "metislib.h"
} }
#include "FindSeparator.h"
namespace gtsam { namespace partition { namespace gtsam { namespace partition {

161
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -25,19 +25,19 @@
namespace gtsam { namespace gtsam {
/** /**
* A class for a measurement predicted by "between(config[key1],config[key2])" * A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type * @tparam VALUE the Value type
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class VALUE> template<class VALUE>
class BetweenFactorEM: public NonlinearFactor { class BetweenFactorEM: public NonlinearFactor {
public: public:
typedef VALUE T; typedef VALUE T;
private: private:
typedef BetweenFactorEM<VALUE> This; typedef BetweenFactorEM<VALUE> This;
typedef gtsam::NonlinearFactor Base; typedef gtsam::NonlinearFactor Base;
@ -56,54 +56,57 @@ namespace gtsam {
bool flag_bump_up_near_zero_probs_; bool flag_bump_up_near_zero_probs_;
/** concept check by type */ /** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T) GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T)
GTSAM_CONCEPT_TESTABLE_TYPE(T)
public: public:
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<BetweenFactorEM> shared_ptr; typedef typename boost::shared_ptr<BetweenFactorEM> shared_ptr;
/** default constructor - only use for serialization */ /** default constructor - only use for serialization */
BetweenFactorEM() {} BetweenFactorEM() {
}
/** Constructor */ /** Constructor */
BetweenFactorEM(Key key1, Key key2, const VALUE& measured, BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, 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) : const double prior_inlier, const double prior_outlier,
Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured), const bool flag_bump_up_near_zero_probs = false) :
model_inlier_(model_inlier), model_outlier_(model_outlier), Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(
prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs){ 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() {} virtual ~BetweenFactorEM() {
}
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
std::cout << s << "BetweenFactorEM(" DefaultKeyFormatter) const {
<< keyFormatter(key1_) << "," std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << ","
<< keyFormatter(key2_) << ")\n"; << keyFormatter(key2_) << ")\n";
measured_.print(" measured: "); measured_.print(" measured: ");
model_inlier_->print(" noise model inlier: "); model_inlier_->print(" noise model inlier: ");
model_outlier_->print(" noise model outlier: "); model_outlier_->print(" noise model outlier: ");
std::cout << "(prior_inlier, prior_outlier_) = (" std::cout << "(prior_inlier, prior_outlier_) = (" << prior_inlier_ << ","
<< prior_inlier_ << ","
<< prior_outlier_ << ")\n"; << prior_outlier_ << ")\n";
// Base::print(s, keyFormatter); // Base::print(s, keyFormatter);
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This *t = dynamic_cast<const This*> (&f); const This *t = dynamic_cast<const This*>(&f);
if(t && Base::equals(f)) if (t && Base::equals(f))
return key1_ == t->key1_ && key2_ == t->key2_ && return key1_ == t->key1_ && key2_ == t->key2_
&&
// model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
// model_outlier_->equals(t->model_outlier_ ) && // model_outlier_->equals(t->model_outlier_ ) &&
prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_); prior_outlier_ == t->prior_outlier_
&& prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
else else
return false; return false;
} }
@ -122,7 +125,8 @@ namespace gtsam {
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
/* This version of linearize recalculates the noise model each time */ /* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const { virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
const gtsam::Values& x) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>(); return boost::shared_ptr<gtsam::JacobianFactor>();
@ -135,10 +139,10 @@ namespace gtsam {
A2 = A[1]; A2 = A[1];
return gtsam::GaussianFactor::shared_ptr( return gtsam::GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size()))); new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
gtsam::noiseModel::Unit::Create(b.size())));
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x, gtsam::Vector whitenedError(const gtsam::Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const { boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
@ -164,31 +168,33 @@ namespace gtsam {
Vector err_wh_outlier = model_outlier_->whiten(err); Vector err_wh_outlier = model_outlier_->whiten(err);
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); Matrix invCov_outlier = model_outlier_->R().transpose()
* model_outlier_->R();
Vector err_wh_eq; Vector err_wh_eq;
err_wh_eq.resize(err_wh_inlier.rows()*2); 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(); err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier)
* err_wh_outlier.array();
if (H){ if (H) {
// stack Jacobians for the two indicators for each of the key // stack Jacobians for the two indicators for each of the key
Matrix H1_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H1); Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
Matrix H1_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H1); Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier); Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
Matrix H2_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H2); Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
Matrix H2_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H2); Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier); Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
(*H)[0].resize(H1_aug.rows(),H1_aug.cols()); (*H)[0].resize(H1_aug.rows(), H1_aug.cols());
(*H)[1].resize(H2_aug.rows(),H2_aug.cols()); (*H)[1].resize(H2_aug.rows(), H2_aug.cols());
(*H)[0] = H1_aug; (*H)[0] = H1_aug;
(*H)[1] = H2_aug; (*H)[1] = H2_aug;
} }
if (debug){ if (debug) {
// std::cout<<"unwhitened error: "<<err[0]<<" "<<err[1]<<" "<<err[2]<<std::endl; // 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_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<<"err_wh_outlier: "<<err_wh_outlier[0]<<" "<<err_wh_outlier[1]<<" "<<err_wh_outlier[2]<<std::endl;
@ -219,7 +225,6 @@ namespace gtsam {
// std::cout<<"===="<<std::endl; // std::cout<<"===="<<std::endl;
} }
return err_wh_eq; return err_wh_eq;
} }
@ -235,28 +240,37 @@ namespace gtsam {
Vector err_wh_outlier = model_outlier_->whiten(err); Vector err_wh_outlier = model_outlier_->whiten(err);
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->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_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant())
double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); * 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){ if (debug) {
std::cout<<"in calcIndicatorProb. err_unwh: "<<err[0]<<", "<<err[1]<<", "<<err[2]<<std::endl; std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", "
std::cout<<"in calcIndicatorProb. err_wh_inlier: "<<err_wh_inlier[0]<<", "<<err_wh_inlier[1]<<", "<<err_wh_inlier[2]<<std::endl; << err[1] << ", " << err[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_inlier: " << err_wh_inlier[0]
std::cout<<"in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "<<err_wh_outlier.dot(err_wh_outlier)<<std::endl; << ", " << 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; std::cout
<< "in calcIndicatorProb. p_inlier, p_outlier before normalization: "
<< p_inlier << ", " << p_outlier << std::endl;
} }
double sumP = p_inlier + p_outlier; double sumP = p_inlier + p_outlier;
p_inlier /= sumP; p_inlier /= sumP;
p_outlier /= sumP; p_outlier /= sumP;
if (flag_bump_up_near_zero_probs_){ if (flag_bump_up_near_zero_probs_) {
// Bump up near-zero probabilities (as in linerFlow.h) // Bump up near-zero probabilities (as in linerFlow.h)
double minP = 0.05; // == 0.1 / 2 indicator variables double minP = 0.05; // == 0.1 / 2 indicator variables
if (p_inlier < minP || p_outlier < minP){ if (p_inlier < minP || p_outlier < minP) {
if (p_inlier < minP) if (p_inlier < minP)
p_inlier = minP; p_inlier = minP;
if (p_outlier < minP) if (p_outlier < minP)
@ -305,16 +319,17 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix get_model_inlier_cov() const { Matrix get_model_inlier_cov() const {
return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); return (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix get_model_outlier_cov() const { Matrix get_model_outlier_cov() const {
return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); return (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ void updateNoiseModels(const gtsam::Values& values,
const gtsam::NonlinearFactorGraph& graph) {
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories /* 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). * (note these are given in the E step, where indicator probabilities are calculated).
* *
@ -328,7 +343,7 @@ namespace gtsam {
std::vector<gtsam::Key> Keys; std::vector<gtsam::Key> Keys;
Keys.push_back(key1_); Keys.push_back(key1_);
Keys.push_back(key2_); Keys.push_back(key2_);
Marginals marginals( graph, values, Marginals::QR ); Marginals marginals(graph, values, Marginals::QR);
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
Matrix cov1 = joint_marginal12(key1_, key1_); Matrix cov1 = joint_marginal12(key1_, key1_);
Matrix cov2 = joint_marginal12(key2_, key2_); Matrix cov2 = joint_marginal12(key2_, key2_);
@ -338,7 +353,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& 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 /* 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). * (note these are given in the E step, where indicator probabilities are calculated).
* *
@ -352,27 +368,30 @@ namespace gtsam {
const T& p2 = values.at<T>(key2_); const T& p2 = values.at<T>(key2_);
Matrix H1, H2; Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x) p1.between(p2, H1, H2); // h(x)
Matrix H; Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows()); H.resize(H1.rows(), H1.rows() + H2.rows());
H << H1, H2; // H = [H1 H2] H << H1, H2; // H = [H1 H2]
Matrix joint_cov; Matrix joint_cov;
joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols());
joint_cov << cov1, cov12, joint_cov << cov1, cov12, cov12.transpose(), cov2;
cov12.transpose(), cov2;
Matrix cov_state = H*joint_cov*H.transpose(); Matrix cov_state = H * joint_cov * H.transpose();
// model_inlier_->print("before:"); // model_inlier_->print("before:");
// update inlier and outlier noise models // update inlier and outlier noise models
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); Matrix covRinlier =
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); (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(); Matrix covRoutlier =
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(
covRoutlier + cov_state);
// model_inlier_->print("after:"); // model_inlier_->print("after:");
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl; // std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
@ -393,16 +412,18 @@ namespace gtsam {
return model_inlier_->R().rows() + model_inlier_->R().cols(); return model_inlier_->R().rows() + model_inlier_->R().cols();
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", ar
& boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; // \class BetweenFactorEM };
// \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_); const T& p2 = values.at<T>(keyB_);
Matrix H1, H2; Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x) p1.between(p2, H1, H2); // h(x)
Matrix H; Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows()); 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 double fpm = 0.5; // fraction of points matched
volatile size_t nm = fpm * n * np; // number of matches 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; cout << "Generating " << nm << " matches" << endl;
boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rn( boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rn(
boost::mt19937(), boost::uniform_int<size_t>(0, N - 1)); 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++) for (size_t k = 0; k < nm; k++)
matches.push_back(Match(rn(), rn())); 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 // 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::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)); 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) { 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 LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3)); return factor.evaluateError(p1, v1, b1, p2, v2).tail(3);
} }
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
@ -64,21 +64,21 @@ int main() {
Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); 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, Rot3 R1(0.487316618, 0.125253866, 0.86419557,
0.580273724, 0.693095498, -0.427669306, 0.580273724, 0.693095498, -0.427669306,
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); 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, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
LieVector Vel2 = Vel1.compose( dv ); Vector3 Vel2 = Vel1 + dv;
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Values values; Values values;

8
gtsam_unstable/timing/timeSFMExpressions.cpp
View File

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

2
matlab/+gtsam/VisualISAMPlot.m
View File

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

2
matlab/+gtsam/VisualISAMStep.m
View File

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

4
matlab/+gtsam/load3D.m
View File

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

7
matlab/+gtsam/plot2DPoints.m
View File

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

6
matlab/+gtsam/plot2DTrajectory.m
View File

@ -33,10 +33,12 @@ else
keys = KeyVector(values.keys); keys = KeyVector(values.keys);
for i = 0:keys.size-1 for i = 0:keys.size-1
key = keys.at(i); key = keys.at(i);
x = values.at(key); try
if isa(x, 'gtsam.Pose2') x = values.atPose2(key);
P = marginals.marginalCovariance(key); P = marginals.marginalCovariance(key);
gtsam.plotPose2(x,linespec(1), P); gtsam.plotPose2(x,linespec(1), P);
catch err
% I guess it's not a Pose2
end end
end end
end end

6
matlab/+gtsam/plot3DPoints.m
View File

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

29
matlab/+gtsam/plot3DTrajectory.m
View File

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

6
matlab/gtsam_examples/PlanarSLAMExample.m
View File

@ -71,9 +71,9 @@ marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals); plot2DTrajectory(result, [], marginals);
plot2DPoints(result, 'b', 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.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).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([-0.6 4.8 -1 1])
axis equal axis equal
view(2) 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); [graph,initial] = load2D(datafile, model);
%% Add a Gaussian prior on a pose in the middle %% 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]); priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]);
graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph 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 for j=1:2
key = symbol('l',j); key = symbol('l',j);
point{j} = sample.at(key); point{j} = sample.atPoint2(key);
Q{j}=marginals.marginalCovariance(key); Q{j}=marginals.marginalCovariance(key);
S{j}=chol(Q{j}); % for sampling S{j}=chol(Q{j}); % for sampling
end end
plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-'); plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-'); plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).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 view(2); axis auto; axis equal
%% Do Sampling on point 2 %% 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 %% Plot Covariance Ellipses
cla; cla;
hold on 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); marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals); plot2DTrajectory(result, [], marginals);

14
matlab/gtsam_examples/Pose2SLAMExample_circle.m
View File

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

2
matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

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

14
matlab/gtsam_examples/Pose3SLAMExample.m
View File

@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses %% Create a hexagon of poses
hexagon = circlePose3(6,1.0); hexagon = circlePose3(6,1.0);
p0 = hexagon.at(0); p0 = hexagon.atPose3(0);
p1 = hexagon.at(1); p1 = hexagon.atPose3(1);
%% create a Pose graph with one equality constraint and one measurement %% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph; fg = NonlinearFactorGraph;
@ -33,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values; initial = Values;
s = 0.10; s = 0.10;
initial.insert(0, p0); initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
%% Plot Initial Estimate %% Plot Initial Estimate
cla 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 %% Plot Initial Estimate
cla cla
first = initial.at(0); first = initial.atPose3(0);
plot3(first.x(),first.y(),first.z(),'r*'); hold on plot3(first.x(),first.y(),first.z(),'r*'); hold on
plot3DTrajectory(initial,'g-',false); plot3DTrajectory(initial,'g-',false);
drawnow; 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))) if ~initial.exists(symbol('l',sf(2)))
% 3D landmarks are stored in camera coordinates: transform % 3D landmarks are stored in camera coordinates: transform
% to world coordinates using the respective initial pose % 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))); world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8)));
initial.insert(symbol('l',sf(2)), world_point); initial.insert(symbol('l',sf(2)), world_point);
end end
@ -54,7 +54,7 @@ toc
%% add a constraint on the starting pose %% add a constraint on the starting pose
key = symbol('x',1); key = symbol('x',1);
first_pose = initial.at(key); first_pose = initial.atPose3(key);
graph.add(NonlinearEqualityPose3(key, first_pose)); graph.add(NonlinearEqualityPose3(key, first_pose));
%% optimize %% 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)); groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
for i=1:3 for i=1:3
graph.add(PriorFactorPose2(i, groundTruth.at(i), model)); graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
end end
%% Initialize to noisy points %% Initialize to noisy points
@ -46,7 +46,7 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
P={}; P={};
for i=1:result.size() for i=1:result.size()
pose_i = result.at(i); pose_i = result.atPose2(i);
CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4)); CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4));
P{i}=marginals.marginalCovariance(i); P{i}=marginals.marginalCovariance(i);
end end

2
matlab/gtsam_tests/testOdometryExample.m
View File

@ -39,5 +39,5 @@ marginals = Marginals(graph, result);
marginals.marginalCovariance(1); marginals.marginalCovariance(1);
%% Check first pose equality %% 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)); 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); marginals = Marginals(graph, result);
%% Check first pose and point equality %% 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)); marginals.marginalCovariance(symbol('x',1));
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); 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)); marginals.marginalCovariance(symbol('l',1));
CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4)); 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); marginals = Marginals(graph, result);
P = marginals.marginalCovariance(1); 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)); 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 %% Create a hexagon of poses
hexagon = circlePose3(6,1.0); hexagon = circlePose3(6,1.0);
p0 = hexagon.at(0); p0 = hexagon.atPose3(0);
p1 = hexagon.at(1); p1 = hexagon.atPose3(1);
%% create a Pose graph with one equality constraint and one measurement %% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph; fg = NonlinearFactorGraph;
@ -33,17 +33,17 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values; initial = Values;
s = 0.10; s = 0.10;
initial.insert(0, p0); initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
%% optimize %% optimize
optimizer = LevenbergMarquardtOptimizer(fg, initial); optimizer = LevenbergMarquardtOptimizer(fg, initial);
result = optimizer.optimizeSafely; 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)); 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 %% Check optimized results, should be equal to ground truth
for i=1:size(truth.cameras,2) 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)) CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end end
for j=1:size(truth.points,2) 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)) CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end end

4
matlab/gtsam_tests/testStereoVOExample.m
View File

@ -61,8 +61,8 @@ optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize(); result = optimizer.optimize();
%% check equality for the first pose and point %% 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)); 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)); 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 end
for i=1:size(truth.cameras,2) 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)) CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end end
for j=1:size(truth.points,2) 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)) CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end end

6
matlab/gtsam_tests/test_gtsam.m
View File

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

2
package_scripts/toolbox_package_unix.sh
View File

@ -61,4 +61,4 @@ if [ $? -ne 0 ]; then
fi fi
# Create package # 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 * @file testPCGSolver.cpp
* @brief Unit tests for PCGSolver class * @brief Unit tests for PCGSolver class
* @author Yong-Dian Jian * @author Yong-Dian Jian
* @date Aug 06, 2014
*/ */
#include <tests/smallExample.h> #include <tests/smallExample.h>
@ -51,6 +52,7 @@ using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
// Test cholesky decomposition
TEST( PCGSolver, llt ) { TEST( PCGSolver, llt ) {
Matrix R = (Matrix(3,3) << Matrix R = (Matrix(3,3) <<
1., -1., -1., 1., -1., -1.,
@ -90,6 +92,7 @@ TEST( PCGSolver, llt ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test Dummy Preconditioner
TEST( PCGSolver, dummy ) TEST( PCGSolver, dummy )
{ {
LevenbergMarquardtParams paramsPCG; LevenbergMarquardtParams paramsPCG;
@ -110,6 +113,7 @@ TEST( PCGSolver, dummy )
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test Block-Jacobi Precondioner
TEST( PCGSolver, blockjacobi ) TEST( PCGSolver, blockjacobi )
{ {
LevenbergMarquardtParams paramsPCG; LevenbergMarquardtParams paramsPCG;
@ -130,6 +134,7 @@ TEST( PCGSolver, blockjacobi )
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test Incremental Subgraph PCG Solver
TEST( PCGSolver, subgraph ) TEST( PCGSolver, subgraph )
{ {
LevenbergMarquardtParams paramsPCG; 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 top = mat.block(0, 0, n, n);
gtsam::SubMatrix block = mat.block(m/4, n/4, m-m/2, n-n/2); 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(" Basic: %1%x%2%\n") % (int)m % (int)n;
cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % 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 % n % 0 % 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 << 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; cout << endl;

2
timing/timePinholeCamera.cpp
View File

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

91
wrap/Argument.cpp
View File

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

49
wrap/Argument.h
View File

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

316
wrap/Class.cpp
View File

@ -33,7 +33,7 @@ using namespace std;
using namespace wrap; 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, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
vector<string>& functionNames) const { vector<string>& functionNames) const {
@ -41,17 +41,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
createNamespaceStructure(namespaces, toolboxPath); createNamespaceStructure(namespaces, toolboxPath);
// open destination classFile // open destination classFile
string classFile = toolboxPath; string classFile = matlabName(toolboxPath);
if (!namespaces.empty())
classFile += "/+" + wrap::qualifiedName("/+", namespaces);
classFile += "/" + name + ".m";
FileWriter proxyFile(classFile, verbose_, "%"); FileWriter proxyFile(classFile, verbose_, "%");
// get the name of actual matlab object // get the name of actual matlab object
const string matlabQualName = qualifiedName("."), matlabUniqueName = const string matlabQualName = qualifiedName(".");
qualifiedName(), cppName = qualifiedName("::"); const string matlabUniqueName = qualifiedName();
const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); const string cppName = qualifiedName("::");
const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); const string matlabBaseName = qualifiedParent.qualifiedName(".");
const string cppBaseName = qualifiedParent.qualifiedName("::");
// emit class proxy code // emit class proxy code
// we want our class to inherit the handle class for memory purposes // 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, pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName,
functionNames); functionNames);
wrapperFile.oss << "\n"; wrapperFile.oss << "\n";
// Regular constructors // 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(); const int id = (int) functionNames.size();
constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(),
id, a); id, args);
const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
cppName, matlabUniqueName, cppBaseName, id, a); cppName, matlabUniqueName, cppBaseName, id, args);
wrapperFile.oss << "\n"; wrapperFile.oss << "\n";
functionNames.push_back(wrapFunctionName); functionNames.push_back(wrapFunctionName);
} }
@ -144,19 +144,14 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
proxyFile.emit(true); proxyFile.emit(true);
} }
/* ************************************************************************* */
string Class::qualifiedName(const string& delim) const {
return ::wrap::qualifiedName(delim, namespaces, name);
}
/* ************************************************************************* */ /* ************************************************************************* */
void Class::pointer_constructor_fragments(FileWriter& proxyFile, void Class::pointer_constructor_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const string& wrapperName, FileWriter& wrapperFile, Str wrapperName,
vector<string>& functionNames) const { vector<string>& functionNames) const {
const string matlabUniqueName = qualifiedName(), cppName = qualifiedName( const string matlabUniqueName = qualifiedName();
"::"); const string cppName = qualifiedName("::");
const string baseCppName = wrap::qualifiedName("::", qualifiedParent); const string baseCppName = qualifiedParent.qualifiedName("::");
const int collectorInsertId = (int) functionNames.size(); const int collectorInsertId = (int) functionNames.size();
const string collectorInsertFunctionName = matlabUniqueName const string collectorInsertFunctionName = matlabUniqueName
@ -247,128 +242,126 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
} }
/* ************************************************************************* */ /* ************************************************************************* */
vector<ArgumentList> expandArgumentListsTemplate( Class Class::expandTemplate(const TemplateSubstitution& ts) const {
const vector<ArgumentList>& argLists, const string& templateArg, Class inst = *this;
const vector<string>& instName, inst.methods = expandMethodTemplate(methods, ts);
const std::vector<string>& expandedClassNamespace, inst.static_methods = expandMethodTemplate(static_methods, ts);
const string& expandedClassName) { inst.constructor = constructor.expandTemplate(ts);
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;
inst.deconstructor.name = inst.name; inst.deconstructor.name = inst.name;
inst.verbose_ = cls.verbose_;
return inst; return inst;
} }
/* ************************************************************************* */ /* ************************************************************************* */
vector<Class> Class::expandTemplate(const string& templateArg, vector<Class> Class::expandTemplate(Str templateArg,
const vector<vector<string> >& instantiations) const { const vector<Qualified>& instantiations) const {
vector<Class> result; vector<Class> result;
BOOST_FOREACH(const vector<string>& instName, instantiations) { BOOST_FOREACH(const Qualified& instName, instantiations) {
const string expandedName = name + instName.back(); Qualified expandedClass = (Qualified) (*this);
Class inst = expandClassTemplate(*this, templateArg, instName, expandedClass.name += instName.name;
this->namespaces, expandedName); const TemplateSubstitution ts(templateArg, instName, expandedClass);
inst.name = expandedName; Class inst = expandTemplate(ts);
inst.name = expandedClass.name;
inst.templateArgs.clear(); inst.templateArgs.clear();
inst.typedefName = qualifiedName("::") + "<" inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::")
+ wrap::qualifiedName("::", instName) + ">"; + ">";
result.push_back(inst); result.push_back(inst);
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Class Class::expandTemplate(const string& templateArg, void Class::addMethod(bool verbose, bool is_const, Str methodName,
const vector<string>& instantiation, const ArgumentList& argumentList, const ReturnValue& returnValue,
const std::vector<string>& expandedClassNamespace, Str templateArgName, const vector<Qualified>& templateArgValues) {
const string& expandedClassName) const { // Check if templated
return expandClassTemplate(*this, templateArg, instantiation, if (!templateArgName.empty() && templateArgValues.size() > 0) {
expandedClassNamespace, expandedClassName); // 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; string result;
BOOST_FOREACH(const string& namesp, namespaces) { BOOST_FOREACH(Str namesp, namespaces) {
result += ("namespace " + namesp + " { "); result += ("namespace " + namesp + " { ");
} }
result += ("typedef " + typedefName + " " + name + ";"); result += ("typedef " + typedefName + " " + name + ";");
@ -379,43 +372,22 @@ std::string Class::getTypedef() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Class::comment_fragment(FileWriter& proxyFile) const { void Class::comment_fragment(FileWriter& proxyFile) const {
proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; proxyFile.oss << "%class " << name << ", see Doxygen page for details\n";
proxyFile.oss proxyFile.oss
<< "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
if (!constructor.args_list.empty()) constructor.comment_fragment(proxyFile);
proxyFile.oss << "%\n%-------Constructors-------\n";
BOOST_FOREACH(ArgumentList argList, constructor.args_list) {
proxyFile.oss << "%";
argList.emit_prototype(proxyFile, name);
proxyFile.oss << "\n";
}
if (!methods.empty()) if (!methods.empty())
proxyFile.oss << "%\n%-------Methods-------\n"; proxyFile.oss << "%\n%-------Methods-------\n";
BOOST_FOREACH(const Methods::value_type& name_m, methods) { BOOST_FOREACH(const Methods::value_type& name_m, methods)
const Method& m = name_m.second; name_m.second.comment_fragment(proxyFile);
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;
}
}
if (!static_methods.empty()) if (!static_methods.empty())
proxyFile.oss << "%\n%-------Static Methods-------\n"; proxyFile.oss << "%\n%-------Static Methods-------\n";
BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods)
const StaticMethod& m = name_m.second; name_m.second.comment_fragment(proxyFile);
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;
}
}
if (hasSerialization) { if (hasSerialization) {
proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%\n%-------Serialization Interface-------\n";
@ -429,23 +401,24 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
/* ************************************************************************* */ /* ************************************************************************* */
void Class::serialization_fragments(FileWriter& proxyFile, void Class::serialization_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const std::string& wrapperName, FileWriter& wrapperFile, Str wrapperName,
std::vector<std::string>& functionNames) const { vector<string>& functionNames) const {
//void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
//{ //{
// typedef boost::shared_ptr<Point3> Shared; // typedef boost::shared_ptr<Point3> Shared;
// checkArguments("string_serialize",nargout,nargin-1,0); // checkArguments("string_serialize",nargout,nargin-1,0);
// Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3"); // 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); // boost::archive::text_oarchive out_archive(out_archive_stream);
// out_archive << *obj; // out_archive << *obj;
// out[0] = wrap< string >(out_archive_stream.str()); // out[0] = wrap< string >(out_archive_stream.str());
//} //}
int serialize_id = functionNames.size(); int serialize_id = functionNames.size();
const string matlabQualName = qualifiedName("."), matlabUniqueName = const string matlabQualName = qualifiedName(".");
qualifiedName(), cppClassName = qualifiedName("::"); const string matlabUniqueName = qualifiedName();
const string cppClassName = qualifiedName("::");
const string wrapFunctionNameSerialize = matlabUniqueName const string wrapFunctionNameSerialize = matlabUniqueName
+ "_string_serialize_" + boost::lexical_cast<string>(serialize_id); + "_string_serialize_" + boost::lexical_cast<string>(serialize_id);
functionNames.push_back(wrapFunctionNameSerialize); functionNames.push_back(wrapFunctionNameSerialize);
@ -469,7 +442,7 @@ void Class::serialization_fragments(FileWriter& proxyFile,
<< ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
// Serialization boilerplate // Serialization boilerplate
wrapperFile.oss << " std::ostringstream out_archive_stream;\n"; wrapperFile.oss << " ostringstream out_archive_stream;\n";
wrapperFile.oss wrapperFile.oss
<< " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; << " boost::archive::text_oarchive out_archive(out_archive_stream);\n";
wrapperFile.oss << " out_archive << *obj;\n"; wrapperFile.oss << " out_archive << *obj;\n";
@ -520,22 +493,23 @@ void Class::serialization_fragments(FileWriter& proxyFile,
/* ************************************************************************* */ /* ************************************************************************* */
void Class::deserialization_fragments(FileWriter& proxyFile, void Class::deserialization_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const std::string& wrapperName, FileWriter& wrapperFile, Str wrapperName,
std::vector<std::string>& functionNames) const { vector<string>& functionNames) const {
//void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
//{ //{
// typedef boost::shared_ptr<Point3> Shared; // typedef boost::shared_ptr<Point3> Shared;
// checkArguments("Point3.string_deserialize",nargout,nargin,1); // checkArguments("Point3.string_deserialize",nargout,nargin,1);
// string serialized = unwrap< string >(in[0]); // 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); // boost::archive::text_iarchive in_archive(in_archive_stream);
// Shared output(new Point3()); // Shared output(new Point3());
// in_archive >> *output; // in_archive >> *output;
// out[0] = wrap_shared_ptr(output,"Point3", false); // out[0] = wrap_shared_ptr(output,"Point3", false);
//} //}
int deserialize_id = functionNames.size(); int deserialize_id = functionNames.size();
const string matlabQualName = qualifiedName("."), matlabUniqueName = const string matlabQualName = qualifiedName(".");
qualifiedName(), cppClassName = qualifiedName("::"); const string matlabUniqueName = qualifiedName();
const string cppClassName = qualifiedName("::");
const string wrapFunctionNameDeserialize = matlabUniqueName const string wrapFunctionNameDeserialize = matlabUniqueName
+ "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id); + "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id);
functionNames.push_back(wrapFunctionNameDeserialize); functionNames.push_back(wrapFunctionNameDeserialize);
@ -553,7 +527,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
// string argument with deserialization boilerplate // string argument with deserialization boilerplate
wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; 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 wrapperFile.oss
<< " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; << " boost::archive::text_iarchive in_archive(in_archive_stream);\n";
wrapperFile.oss << " Shared output(new " << cppClassName << "());\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"); //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal");
return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \""
+ 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";
}
/* ************************************************************************* */ /* ************************************************************************* */

98
wrap/Class.h
View File

@ -19,66 +19,118 @@
#pragma once #pragma once
#include <string>
#include <map>
#include "Constructor.h" #include "Constructor.h"
#include "Deconstructor.h" #include "Deconstructor.h"
#include "Method.h" #include "Method.h"
#include "StaticMethod.h" #include "StaticMethod.h"
#include "TypeAttributesTable.h" #include "TypeAttributesTable.h"
#include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp>
#include <string>
#include <map>
namespace wrap { namespace wrap {
/// Class has name, constructors, methods /// Class has name, constructors, methods
struct Class { class Class: public Qualified {
typedef std::map<std::string, Method> Methods; typedef std::map<std::string, Method> Methods;
Methods methods; ///< Class methods
public:
typedef const std::string& Str;
typedef std::map<std::string, StaticMethod> StaticMethods; 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 // Then the instance variables are set directly by the Module constructor
std::string name; ///< Class name
std::vector<std::string> templateArgs; ///< Template arguments 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] 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 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 isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports
bool hasSerialization; ///< Whether we should create the serialization functions 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 Qualified qualifiedParent; ///< The *single* parent
Methods methods; ///< Class methods
StaticMethods static_methods; ///< Static methods StaticMethods static_methods; ///< Static methods
std::vector<std::string> namespaces; ///< Stack of namespaces
Constructor constructor; ///< Class constructors Constructor constructor; ///< Class constructors
Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object
bool verbose_; ///< verbose flag 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 // 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, void matlab_proxy(Str toolboxPath, Str wrapperName,
FileWriter& wrapperFile, std::vector<std::string>& functionNames) const; ///< emit proxy class 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; std::string getTypedef() const;
// Returns the string for an export flag /// Returns the string for an export flag
std::string getSerializationExport() const; 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, 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, 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: private:
void pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const std::string& wrapperName, std::vector<std::string>& functionNames) const;
void pointer_constructor_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, Str wrapperName,
std::vector<std::string>& functionNames) const;
void comment_fragment(FileWriter& proxyFile) const; void comment_fragment(FileWriter& proxyFile) const;
}; };

72
wrap/Constructor.cpp
View File

@ -29,52 +29,55 @@
using namespace std; using namespace std;
using namespace wrap; using namespace wrap;
/* ************************************************************************* */ /* ************************************************************************* */
string Constructor::matlab_wrapper_name(const string& className) const { string Constructor::matlab_wrapper_name(Str className) const {
string str = "new_" + className; string str = "new_" + className;
return str; return str;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperName, void Constructor::proxy_fragment(FileWriter& file,
bool hasParent, const int id, const ArgumentList args) const { const std::string& wrapperName, bool hasParent, const int id,
const ArgumentList args) const {
size_t nrArgs = args.size(); size_t nrArgs = args.size();
// check for number of arguments... // check for number of arguments...
file.oss << " elseif nargin == " << nrArgs; file.oss << " elseif nargin == " << nrArgs;
if (nrArgs>0) file.oss << " && "; if (nrArgs > 0)
file.oss << " && ";
// ...and their types // ...and their types
bool first = true; bool first = true;
for(size_t i=0;i<nrArgs;i++) { for (size_t i = 0; i < nrArgs; i++) {
if (!first) file.oss << " && "; if (!first)
file.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')"; file.oss << " && ";
first=false; file.oss << "isa(varargin{" << i + 1 << "},'" << args[i].matlabClass(".")
<< "')";
first = false;
} }
// emit code for calling constructor // emit code for calling constructor
if(hasParent) if (hasParent)
file.oss << "\n [ my_ptr, base_ptr ] = "; file.oss << "\n [ my_ptr, base_ptr ] = ";
else else
file.oss << "\n my_ptr = "; file.oss << "\n my_ptr = ";
file.oss << wrapperName << "(" << id; file.oss << wrapperName << "(" << id;
// emit constructor arguments // emit constructor arguments
for(size_t i=0;i<nrArgs;i++) { for (size_t i = 0; i < nrArgs; i++) {
file.oss << ", "; file.oss << ", ";
file.oss << "varargin{" << i+1 << "}"; file.oss << "varargin{" << i + 1 << "}";
} }
file.oss << ");\n"; file.oss << ");\n";
} }
/* ************************************************************************* */ /* ************************************************************************* */
string Constructor::wrapper_fragment(FileWriter& file, string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName,
const string& cppClassName, Str matlabUniqueName, Str cppBaseClassName, int id,
const string& matlabUniqueName,
const string& cppBaseClassName,
int id,
const ArgumentList& al) const { 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 << "{\n";
file.oss << " mexAtExit(&_deleteAllObjects);\n"; file.oss << " mexAtExit(&_deleteAllObjects);\n";
//Typedef boost::shared_ptr //Typedef boost::shared_ptr
@ -82,22 +85,29 @@ string Constructor::wrapper_fragment(FileWriter& file,
file.oss << "\n"; file.oss << "\n";
//Check to see if there will be any arguments and remove {} for consiseness //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 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"; file.oss << " collector_" << matlabUniqueName << ".insert(self);\n";
if(verbose_) if (verbose_)
file.oss << " std::cout << \"constructed \" << self << std::endl;" << endl; file.oss << " std::cout << \"constructed \" << self << std::endl;" << endl;
file.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl; file.oss
file.oss << " *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;" << endl; << " 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 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 << "\n";
file.oss << " typedef boost::shared_ptr<" << cppBaseClassName << "> SharedBase;\n"; file.oss << " typedef boost::shared_ptr<" << cppBaseClassName
file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; << "> SharedBase;\n";
file.oss << " *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);\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; 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 #pragma once
#include "Argument.h" #include "OverloadedFunction.h"
#include <string> #include <string>
#include <vector> #include <vector>
@ -26,42 +26,64 @@
namespace wrap { namespace wrap {
// Constructor class // Constructor class
struct Constructor { struct Constructor: public OverloadedFunction {
typedef const std::string& Str;
/// Constructor creates an empty class /// Constructor creates an empty class
Constructor(bool verbose = false) : Constructor(bool verbose = false) {
verbose_(verbose) { verbose_ = verbose;
} }
// Then the instance variables are set directly by the Module constructor Constructor expandTemplate(const TemplateSubstitution& ts) const {
std::vector<ArgumentList> args_list; Constructor inst = *this;
std::string name; inst.argLists_ = expandArgumentListsTemplate(ts);
bool verbose_; inst.name_ = ts.expandedClassName();
return inst;
}
// MATLAB code generation // MATLAB code generation
// toolboxPath is main toolbox directory, e.g., ../matlab // toolboxPath is main toolbox directory, e.g., ../matlab
// classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m
/// wrapper name /// 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., * Create fragment to select constructor in proxy class, e.g.,
* if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end
*/ */
void proxy_fragment(FileWriter& file, const std::string& wrapperName, void proxy_fragment(FileWriter& file, Str wrapperName, bool hasParent,
bool hasParent, const int id, const ArgumentList args) const; const int id, const ArgumentList args) const;
/// cpp wrapper /// cpp wrapper
std::string wrapper_fragment(FileWriter& file, std::string wrapper_fragment(FileWriter& file, Str cppClassName,
const std::string& cppClassName, const std::string& matlabUniqueName, Str matlabUniqueName, Str cppBaseClassName, int id,
const std::string& cppBaseClassName, int id,
const ArgumentList& al) const; const ArgumentList& al) const;
/// constructor function /// constructor function
void generate_construct(FileWriter& file, const std::string& cppClassName, void generate_construct(FileWriter& file, Str cppClassName,
std::vector<ArgumentList>& args_list) const; 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 } // \namespace wrap

27
wrap/FileWriter.cpp
View File

@ -15,14 +15,15 @@ using namespace std;
using namespace wrap; using namespace wrap;
/* ************************************************************************* */ /* ************************************************************************* */
FileWriter::FileWriter(const string& filename, bool verbose, const string& comment_str) FileWriter::FileWriter(const string& filename, bool verbose,
: verbose_(verbose),filename_(filename), comment_str_(comment_str) const string& comment_str) :
{ verbose_(verbose), filename_(filename), comment_str_(comment_str) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void FileWriter::emit(bool add_header, bool force_overwrite) const { 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 // read in file if it exists
string existing_contents; string existing_contents;
bool file_exists = true; 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 // Only write a file if it is new, an update, or overwrite is forced
string new_contents = oss.str(); string new_contents = oss.str();
if (force_overwrite || !file_exists || existing_contents != new_contents) { 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 // Binary to use LF line endings instead of CRLF
if (!ofs) throw CantOpenFile(filename_); ofstream ofs(filename_.c_str(), ios::binary);
if (!ofs)
throw CantOpenFile(filename_);
// dump in stringstream // dump in stringstream
ofs << new_contents; ofs << new_contents;
ofs.close(); 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; 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 ArgumentList& args, const ReturnValue& retVal,
const StrVec& ns_stack) { const Qualified& instName, bool verbose) {
this->verbose_ = verbose; string name(overload.name);
this->name = name; FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose);
this->argLists.push_back(args); overloads.push_back(overload);
this->returnVals.push_back(retVal);
this->namespaces.push_back(ns_stack);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GlobalFunction::matlab_proxy(const std::string& toolboxPath, void GlobalFunction::matlab_proxy(const string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes, const string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const { FileWriter& file, vector<string>& functionNames) const {
// cluster overloads with same namespace // cluster overloads with same namespace
// create new GlobalFunction structures around namespaces - same namespaces and names are overloads // create new GlobalFunction structures around namespaces - same namespaces and names are overloads
// map of namespace to global function // map of namespace to global function
typedef map<string, GlobalFunction> GlobalFunctionMap; typedef map<string, GlobalFunction> GlobalFunctionMap;
GlobalFunctionMap grouped_functions; GlobalFunctionMap grouped_functions;
for (size_t i = 0; i < namespaces.size(); ++i) { for (size_t i = 0; i < overloads.size(); ++i) {
StrVec ns = namespaces.at(i); Qualified overload = overloads.at(i);
string str_ns = qualifiedName("", ns, ""); // use concatenated namespaces as key
ReturnValue ret = returnVals.at(i); string str_ns = qualifiedName("", overload.namespaces);
ArgumentList args = argLists.at(i); const ReturnValue& ret = returnValue(i);
const ArgumentList& args = argumentList(i);
if (!grouped_functions.count(str_ns)) grouped_functions[str_ns].addOverload(overload, args, ret);
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);
} }
size_t lastcheck = grouped_functions.size(); 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, void GlobalFunction::generateSingleFunction(const string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes, const string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const { FileWriter& file, vector<string>& functionNames) const {
// create the folder for the namespace // create the folder for the namespace
const StrVec& ns = namespaces.front(); const Qualified& overload1 = overloads.front();
createNamespaceStructure(ns, toolboxPath); createNamespaceStructure(overload1.namespaces, toolboxPath);
// open destination mfunctionFileName // open destination mfunctionFileName
string mfunctionFileName = toolboxPath; string mfunctionFileName = overload1.matlabName(toolboxPath);
if (!ns.empty())
mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns);
mfunctionFileName += "/" + name + ".m";
FileWriter mfunctionFile(mfunctionFileName, verbose_, "%"); FileWriter mfunctionFile(mfunctionFileName, verbose_, "%");
// get the name of actual matlab object // get the name of actual matlab object
const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName = const string matlabQualName = overload1.qualifiedName(".");
qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name); 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) { for (size_t i = 0; i < nrOverloads(); ++i) {
const ArgumentList& args = argLists[overload]; const ArgumentList& args = argumentList(i);
const ReturnValue& returnVal = returnVals[overload]; const ReturnValue& returnVal = returnValue(i);
const int id = functionNames.size(); const int id = functionNames.size();
// Output proxy matlab code // Output proxy matlab code
mfunctionFile.oss << " " << (overload == 0 ? "" : "else"); mfunctionFile.oss << " " << (i == 0 ? "" : "else");
argLists[overload].emit_conditional_call(mfunctionFile, args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this"
returnVals[overload], wrapperName, id, true); // true omits "this"
// Output C++ wrapper code // 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 args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
// call method with default type and wrap result // call method with default type and wrap result
if (returnVal.type1 != "void") if (returnVal.type1.name != "void")
returnVal.wrap_result(cppName + "(" + args.names() + ")", file, returnVal.wrap_result(cppName + "(" + args.names() + ")", file,
typeAttributes); typeAttributes);
else else
@ -137,6 +127,11 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
mfunctionFile.emit(true); mfunctionFile.emit(true);
} }
/* ************************************************************************* */
void GlobalFunction::python_wrapper(FileWriter& wrapperFile) const {
wrapperFile.oss << "def(\"" << name_ << "\", " << name_ << ");\n";
}
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace wrap } // \namespace wrap

36
wrap/GlobalFunction.h
View File

@ -9,43 +9,35 @@
#pragma once #pragma once
#include "Argument.h" #include "FullyOverloadedFunction.h"
#include "ReturnValue.h"
namespace wrap { namespace wrap {
struct GlobalFunction { struct GlobalFunction: public FullyOverloadedFunction {
typedef std::vector<std::string> StrVec; std::vector<Qualified> overloads; ///< Stack of qualified names
bool verbose_; // adds an overloaded version of this function,
std::string name; void addOverload(const Qualified& overload, const ArgumentList& args,
const ReturnValue& retVal, const Qualified& instName = Qualified(),
bool verbose = false);
// each overload, regardless of namespace void verifyArguments(const std::vector<std::string>& validArgs) const {
std::vector<ArgumentList> argLists; ///< arugments for each overload SignatureOverloads::verifyArguments(validArgs, name_);
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) {
} }
// Used to reconstruct void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
GlobalFunction(const std::string& name_, bool verbose = true) : SignatureOverloads::verifyReturnTypes(validtypes, name_);
verbose_(verbose), name(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 // codegen function called from Module to build the cpp and matlab versions of the function
void matlab_proxy(const std::string& toolboxPath, void matlab_proxy(const std::string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes, const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const; FileWriter& file, std::vector<std::string>& functionNames) const;
// emit python wrapper
void python_wrapper(FileWriter& wrapperFile) const;
private: private:
// Creates a single global function - all in same namespace // 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; using namespace wrap;
/* ************************************************************************* */ /* ************************************************************************* */
void Method::addOverload(bool verbose, bool is_const, const std::string& name, bool Method::addOverload(Str name, const ArgumentList& args,
const ArgumentList& args, const ReturnValue& retVal) { const ReturnValue& retVal, bool is_const, const Qualified& instName,
this->verbose_ = verbose; bool verbose) {
this->is_const_ = is_const; bool first = StaticMethod::addOverload(name, args, retVal, instName, verbose);
this->name = name; if (first)
this->argLists.push_back(args); is_const_ = is_const;
this->returnVals.push_back(retVal); 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, void Method::proxy_header(FileWriter& proxyFile) const {
const string& cppClassName, const std::string& matlabQualName, proxyFile.oss << " function varargout = " << matlabName()
const std::string& matlabUniqueName, const string& wrapperName, << "(this, varargin)\n";
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";
} }
/* ************************************************************************* */ /* ************************************************************************* */
string Method::wrapper_fragment(FileWriter& file, const string& cppClassName, string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
const string& matlabUniqueName, int overload, int id, Str matlabUniqueName, const ArgumentList& args,
const TypeAttributesTable& typeAttributes) const { const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
const Qualified& instName) 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;
// check arguments // check arguments
// extra argument obj -> nargin-1 is passed ! // extra argument obj -> nargin-1 is passed !
// example: checkArguments("equals",nargout,nargin-1,2); // example: checkArguments("equals",nargout,nargin-1,2);
file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1,"
<< args.size() << ");\n"; << args.size() << ");\n";
// get class pointer // get class pointer
// example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test"); // 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; << ">(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 // call method and wrap result
// example: out[0]=wrap<bool>(self->return_field(t)); // example: out[0]=wrap<bool>(obj->return_field(t));
if (returnVal.type1 != "void") string expanded = "obj->" + name_;
returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file, if (!instName.empty())
typeAttributes); expanded += ("<" + instName.qualifiedName("::") + ">");
else
file.oss << " obj->" + name + "(" + args.names() + ");\n";
// finish return expanded;
file.oss << "}\n";
return wrapFunctionName;
} }
/* ************************************************************************* */ /* ************************************************************************* */

63
wrap/Method.h
View File

@ -18,49 +18,46 @@
#pragma once #pragma once
#include "Argument.h" #include "StaticMethod.h"
#include "ReturnValue.h"
#include "TypeAttributesTable.h"
#include <string>
#include <list>
namespace wrap { namespace wrap {
/// Method class /// 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_; 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 public:
// 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);
// MATLAB code generation typedef const std::string& Str;
// classPath is class directory, e.g., ../matlab/@Point2
void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, bool addOverload(Str name, const ArgumentList& args,
const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, const ReturnValue& retVal, bool is_const, const Qualified& instName =
const std::string& wrapperName, const TypeAttributesTable& typeAttributes, Qualified(), bool verbose = false);
std::vector<std::string>& functionNames) const;
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: private:
std::string wrapper_fragment(FileWriter& file,
const std::string& cppClassName, // Emit method header
const std::string& matlabUniqueName, void proxy_header(FileWriter& proxyFile) const;
int overload,
int id, virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper Str matlabUniqueName, const ArgumentList& args,
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
const Qualified& instName) const;
}; };
} // \namespace wrap } // \namespace wrap

421
wrap/Module.cpp
View File

@ -62,15 +62,21 @@ 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 a number of template arguments were given, generate a number of expanded
if(instantiations.empty()) { // 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); classes.push_back(cls);
} else { } else {
vector<Class> classInstantiations = cls.expandTemplate(templateArgument, instantiations); if (cls.templateArgs.size() != 1)
BOOST_FOREACH(const Class& c, classInstantiations) { 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); classes.push_back(c);
} }
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -94,28 +100,9 @@ Module::Module(const string& interfacePath,
/* ************************************************************************* */ /* ************************************************************************* */
void Module::parseMarkup(const std::string& data) { 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. // The one with postfix 0 are used to reset the variables after parse.
string methodName, methodName0; vector<string> namespaces; // current namespace tag
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 = "";
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
// Grammar with actions that build the Class object. Actions are // 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_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 = Rule argEigenType_p =
eigenType_p[assign_a(arg.type)]; eigenType_p[assign_a(arg.type.name)];
Rule eigenRef_p = Rule eigenRef_p =
!str_p("const") [assign_a(arg.is_const,true)] >> !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)]; ch_p('&') [assign_a(arg.is_ref,true)];
Rule classArg_p = Rule classArg_p =
!str_p("const") [assign_a(arg.is_const,true)] >> !str_p("const") [assign_a(arg.is_const,true)] >>
*namespace_arg_p >> *namespace_arg_p >>
className_p[assign_a(arg.type)] >> className_p[assign_a(arg.type.name)] >>
!ch_p('&')[assign_a(arg.is_ref,true)]; !ch_p('&')[assign_a(arg.is_ref,true)];
Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; 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 = Rule classParent_p =
*(namespace_name_p[push_back_a(cls.qualifiedParent)] >> str_p("::")) >> *(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >>
className_p[push_back_a(cls.qualifiedParent)]; className_p[assign_a(cls.qualifiedParent.name)];
Rule templateInstantiation_p = // parse "gtsam::Pose2" and add to templateArgValues
(*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >> Qualified templateArgValue;
className_p[push_back_a(templateInstantiationNamespace)]) vector<Qualified> templateArgValues;
[push_back_a(templateInstantiations, templateInstantiationNamespace)] Rule templateArgValue_p =
[clear_a(templateInstantiationNamespace)]; (*(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") >> (str_p("template") >>
'<' >> name_p[assign_a(templateArgument)] >> '=' >> '{' >> '<' >> name_p[assign_a(templateArgName)] >> '=' >>
!(templateInstantiation_p >> *(',' >> templateInstantiation_p)) >> '{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >>
'}' >> '>') '>');
[push_back_a(cls.templateArgs, templateArgument)];
// parse "gtsam::Pose2" and add to singleInstantiation.typeList
TemplateInstantiationTypedef singleInstantiation;
Rule templateSingleInstantiationArg_p = Rule templateSingleInstantiationArg_p =
(*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >> (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >>
className_p[push_back_a(templateInstantiationNamespace)]) className_p[assign_a(templateArgValue.name)])
[push_back_a(singleInstantiation.typeList, templateInstantiationNamespace)] [push_back_a(singleInstantiation.typeList, templateArgValue)]
[clear_a(templateInstantiationNamespace)]; [clear_a(templateArgValue)];
// typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
TemplateInstantiationTypedef singleInstantiation0;
Rule templateSingleInstantiation_p = Rule templateSingleInstantiation_p =
(str_p("typedef") >> (str_p("typedef") >>
*(namespace_name_p[push_back_a(singleInstantiation.classNamespaces)] >> str_p("::")) >> *(namespace_name_p[push_back_a(singleInstantiation.class_.namespaces)] >> str_p("::")) >>
className_p[assign_a(singleInstantiation.className)] >> className_p[assign_a(singleInstantiation.class_.name)] >>
'<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >> '<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >>
'>' >> '>' >>
className_p[assign_a(singleInstantiation.name)] >> className_p[assign_a(singleInstantiation.name)] >>
@ -197,14 +196,16 @@ void Module::parseMarkup(const std::string& data) {
[push_back_a(templateInstantiationTypedefs, singleInstantiation)] [push_back_a(templateInstantiationTypedefs, singleInstantiation)]
[assign_a(singleInstantiation, singleInstantiation0)]; [assign_a(singleInstantiation, singleInstantiation0)];
// template<POSE, POINT>
Rule templateList_p = Rule templateList_p =
(str_p("template") >> (str_p("template") >>
'<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >> '<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >>
'>'); '>');
// NOTE: allows for pointers to all types // NOTE: allows for pointers to all types
ArgumentList args;
Rule argument_p = 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)] >> !ch_p('*')[assign_a(arg.is_ptr,true)]
>> name_p[assign_a(arg.name)]) >> name_p[assign_a(arg.name)])
[push_back_a(args, arg)] [push_back_a(args, arg)]
@ -212,114 +213,111 @@ void Module::parseMarkup(const std::string& data) {
Rule argumentList_p = !argument_p >> * (',' >> argument_p); Rule argumentList_p = !argument_p >> * (',' >> argument_p);
// parse class constructor
Constructor constructor0(verbose), constructor(verbose);
Rule constructor_p = Rule constructor_p =
(className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p)
[push_back_a(constructor.args_list, args)] [bl::bind(&Constructor::push_back, bl::var(constructor), bl::var(args))]
[assign_a(args,args0)]; [clear_a(args)];
//[assign_a(constructor.args,args)]
//[assign_a(constructor.name,cls.name)]
//[push_back_a(cls.constructors, constructor)]
//[assign_a(constructor,constructor0)];
vector<string> namespaces_return; /// namespace for current return type
Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); 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 // 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 ReturnType::return_category RETURN_EIGEN = ReturnType::EIGEN;
static const ReturnValue::return_category RETURN_BASIS = ReturnValue::BASIS; static const ReturnType::return_category RETURN_BASIS = ReturnType::BASIS;
static const ReturnValue::return_category RETURN_CLASS = ReturnValue::CLASS; static const ReturnType::return_category RETURN_CLASS = ReturnType::CLASS;
static const ReturnValue::return_category RETURN_VOID = ReturnValue::VOID; static const ReturnType::return_category RETURN_VOID = ReturnType::VOID;
Rule returnType1_p = ReturnType retType0, retType;
(basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_BASIS)]) | Rule returnType_p =
((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)] (basisType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_BASIS)]) |
>> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_CLASS)]) >> ((*namespace_ret_p)[assign_a(retType.namespaces, namespaces_return)][clear_a(namespaces_return)]
!ch_p('*')[assign_a(retVal.isPtr1,true)]) | >> (className_p[assign_a(retType.name)][assign_a(retType.category, RETURN_CLASS)]) >>
(eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_EIGEN)]); !ch_p('*')[assign_a(retType.isPtr,true)]) |
(eigenType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_EIGEN)]);
Rule returnType2_p = ReturnValue retVal0, retVal;
(basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_BASIS)]) | Rule returnType1_p = returnType_p[assign_a(retVal.type1,retType)][assign_a(retType,retType0)];
((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)] Rule returnType2_p = returnType_p[assign_a(retVal.type2,retType)][assign_a(retType,retType0)];
>> (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)]);
Rule pair_p = Rule pair_p =
(str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>')
[assign_a(retVal.isPair,true)]; [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 | '_')]; 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 = Rule method_p =
(returnType_p >> methodName_p[assign_a(methodName)] >> !templateArgValues_p >>
(returnValue_p >> methodName_p[assign_a(methodName)] >>
'(' >> argumentList_p >> ')' >> '(' >> argumentList_p >> ')' >>
!str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p) !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p)
[bl::bind(&Method::addOverload, [bl::bind(&Class::addMethod, bl::var(cls), verbose, bl::var(isConst),
bl::var(cls.methods)[bl::var(methodName)], bl::var(methodName), bl::var(args), bl::var(retVal),
verbose, bl::var(templateArgName), bl::var(templateArgValues))]
bl::var(isConst), [assign_a(retVal,retVal0)]
bl::var(methodName), [clear_a(args)]
bl::var(args), [clear_a(templateArgValues)]
bl::var(retVal))] [assign_a(isConst,isConst0)];
[assign_a(isConst,isConst0)]
[assign_a(methodName,methodName0)]
[assign_a(args,args0)]
[assign_a(retVal,retVal0)];
Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
Rule static_method_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) '(' >> argumentList_p >> ')' >> ';' >> *comments_p)
[bl::bind(&StaticMethod::addOverload, [bl::bind(&StaticMethod::addOverload,
bl::var(cls.static_methods)[bl::var(methodName)], bl::var(cls.static_methods)[bl::var(methodName)],
verbose, bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)]
bl::var(methodName), [assign_a(retVal,retVal0)]
bl::var(args), [clear_a(args)];
bl::var(retVal))]
[assign_a(methodName,methodName0)]
[assign_a(args,args0)]
[assign_a(retVal,retVal0)];
Rule functions_p = constructor_p | method_p | static_method_p; Rule functions_p = constructor_p | method_p | static_method_p;
// parse a full class
vector<Qualified> templateInstantiations;
Rule class_p = Rule class_p =
(str_p("")[assign_a(cls,cls0)]) eps_p[assign_a(cls,cls0)]
>> (!(templateInstantiations_p | templateList_p) >> (!(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("virtual")[assign_a(cls.isVirtual, true)])
>> str_p("class") >> str_p("class")
>> className_p[assign_a(cls.name)] >> className_p[assign_a(cls.name)]
>> ((':' >> classParent_p >> '{') | '{') >> ((':' >> classParent_p >> '{') | '{')
>> *(functions_p | comments_p) >> *(functions_p | comments_p)
>> str_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.constructor, constructor)]
[assign_a(cls.namespaces, namespaces)] [assign_a(cls.namespaces, namespaces)]
[assign_a(deconstructor.name,cls.name)] [assign_a(cls.deconstructor.name,cls.name)]
[assign_a(cls.deconstructor, deconstructor)] [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls),
[bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), bl::var(templateArgument), bl::var(templateInstantiations))] bl::var(templateInstantiations))]
[assign_a(deconstructor,deconstructor0)] [clear_a(templateInstantiations)]
[assign_a(constructor, constructor0)] [assign_a(constructor, constructor0)]
[assign_a(cls,cls0)] [assign_a(cls,cls0)];
[clear_a(templateArgument)]
[clear_a(templateInstantiations)];
// parse a global function
Qualified globalFunction;
Rule global_function_p = Rule global_function_p =
(returnType_p >> staticMethodName_p[assign_a(methodName)] >> (returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >>
'(' >> argumentList_p >> ')' >> ';' >> *comments_p) '(' >> argumentList_p >> ')' >> ';' >> *comments_p)
[assign_a(globalFunction.namespaces,namespaces)]
[bl::bind(&GlobalFunction::addOverload, [bl::bind(&GlobalFunction::addOverload,
bl::var(global_functions)[bl::var(methodName)], bl::var(global_functions)[bl::var(globalFunction.name)],
verbose, bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)]
bl::var(methodName), [assign_a(retVal,retVal0)]
bl::var(args), [clear_a(globalFunction)]
bl::var(retVal), [clear_a(args)];
bl::var(namespaces))]
[assign_a(methodName,methodName0)]
[assign_a(args,args0)]
[assign_a(retVal,retVal0)];
Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>');
@ -340,6 +338,8 @@ void Module::parseMarkup(const std::string& data) {
#pragma clang diagnostic pop #pragma clang diagnostic pop
#endif #endif
// parse forward declaration
ForwardDeclaration fwDec0, fwDec;
Rule forward_declaration_p = Rule forward_declaration_p =
!(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) !(str_p("virtual")[assign_a(fwDec.isVirtual, true)])
>> str_p("class") >> str_p("class")
@ -367,7 +367,7 @@ void Module::parseMarkup(const std::string& data) {
BOOST_SPIRIT_DEBUG_NODE(returnType2_p); BOOST_SPIRIT_DEBUG_NODE(returnType2_p);
BOOST_SPIRIT_DEBUG_NODE(pair_p); BOOST_SPIRIT_DEBUG_NODE(pair_p);
BOOST_SPIRIT_DEBUG_NODE(void_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(methodName_p);
BOOST_SPIRIT_DEBUG_NODE(method_p); BOOST_SPIRIT_DEBUG_NODE(method_p);
BOOST_SPIRIT_DEBUG_NODE(class_p); BOOST_SPIRIT_DEBUG_NODE(class_p);
@ -388,125 +388,41 @@ void Module::parseMarkup(const std::string& data) {
} }
// Post-process classes for serialization markers // Post-process classes for serialization markers
BOOST_FOREACH(Class& cls, classes) { BOOST_FOREACH(Class& cls, classes)
Class::Methods::iterator serializable_it = cls.methods.find("serializable"); cls.erase_serialization();
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);
}
}
// Explicitly add methods to the classes from parents so it shows in documentation // Explicitly add methods to the classes from parents so it shows in documentation
BOOST_FOREACH(Class& cls, classes) BOOST_FOREACH(Class& cls, classes)
{ cls.appendInheritedMethods(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);
// Expand templates - This is done first so that template instantiations are // Expand templates - This is done first so that template instantiations are
// counted in the list of valid types, have their attributes and dependencies // counted in the list of valid types, have their attributes and dependencies
// checked, etc. // checked, etc.
vector<Class> expandedClasses = ExpandTypedefInstantiations(classes, templateInstantiationTypedefs); expandedClasses = ExpandTypedefInstantiations(classes,
templateInstantiationTypedefs);
// Dependency check list // 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 // Check that all classes have been defined somewhere
verifyArguments<GlobalFunction>(validTypes, global_functions); verifyArguments<GlobalFunction>(validTypes, global_functions);
verifyReturnTypes<GlobalFunction>(validTypes, global_functions); verifyReturnTypes<GlobalFunction>(validTypes, global_functions);
bool hasSerialiable = false; hasSerialiable = false;
BOOST_FOREACH(const Class& cls, expandedClasses) { BOOST_FOREACH(const Class& cls, expandedClasses)
hasSerialiable |= cls.isSerializable; cls.verifyAll(validTypes,hasSerialiable);
// 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("::"));
}
// Create type attributes table and check validity // Create type attributes table and check validity
TypeAttributesTable typeAttributes;
typeAttributes.addClasses(expandedClasses); typeAttributes.addClasses(expandedClasses);
typeAttributes.addForwardDeclarations(forward_declarations); typeAttributes.addForwardDeclarations(forward_declarations);
typeAttributes.checkValidity(expandedClasses); typeAttributes.checkValidity(expandedClasses);
}
/* ************************************************************************* */
void Module::matlab_code(const string& toolboxPath) const {
fs::create_directories(toolboxPath);
// create the unified .cpp switch file // create the unified .cpp switch file
const string wrapperName = name + "_wrapper"; 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 // Generate includes while avoiding redundant includes
generateIncludes(wrapperFile); 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 // create typedef classes - we put this at the top of the wrap file so that
BOOST_FOREACH(const Class& cls, expandedClasses) { // collectors and method arguments can use these typedefs
BOOST_FOREACH(const Class& cls, expandedClasses)
if(!cls.typedefName.empty()) if(!cls.typedefName.empty())
wrapperFile.oss << cls.getTypedef() << "\n"; wrapperFile.oss << cls.getTypedef() << "\n";
}
wrapperFile.oss << "\n"; wrapperFile.oss << "\n";
// Generate boost.serialization export flags (needs typedefs from above) // Generate boost.serialization export flags (needs typedefs from above)
if (hasSerialiable) { if (hasSerialiable) {
BOOST_FOREACH(const Class& cls, expandedClasses) { BOOST_FOREACH(const Class& cls, expandedClasses)
if(cls.isSerializable) if(cls.isSerializable)
wrapperFile.oss << cls.getSerializationExport() << "\n"; wrapperFile.oss << cls.getSerializationExport() << "\n";
}
wrapperFile.oss << "\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 vector<string> functionNames; // Function names stored by index for switch
// create proxy class and wrapper code // 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); cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
}
// create matlab files and wrapper code for global functions // 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); p.second.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
}
// finish wrapper file // finish wrapper file
wrapperFile.oss << "\n"; wrapperFile.oss << "\n";
@ -567,28 +480,24 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
wrapperFile.emit(true); 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 { 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 << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\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);
}
/* ************************************************************************* */

50
wrap/Module.h
View File

@ -37,6 +37,7 @@ struct Module {
typedef std::map<std::string, GlobalFunction> GlobalFunctions; typedef std::map<std::string, GlobalFunction> GlobalFunctions;
typedef std::map<std::string, Method> Methods; typedef std::map<std::string, Method> Methods;
// Filled during parsing:
std::string name; ///< module name std::string name; ///< module name
bool verbose; ///< verbose flag bool verbose; ///< verbose flag
std::vector<Class> classes; ///< list of classes std::vector<Class> classes; ///< list of classes
@ -45,35 +46,44 @@ struct Module {
std::vector<std::string> includes; ///< Include statements std::vector<std::string> includes; ///< Include statements
GlobalFunctions global_functions; GlobalFunctions global_functions;
// After parsing:
std::vector<Class> expandedClasses;
bool hasSerialiable;
TypeAttributesTable typeAttributes;
/// constructor that parses interface file /// constructor that parses interface file
Module(const std::string& interfacePath, Module(const std::string& interfacePath, const std::string& moduleName,
const std::string& moduleName, bool enable_verbose = true);
bool enable_verbose=true);
/// Dummy constructor that does no parsing - use only for testing /// Dummy constructor that does no parsing - use only for testing
Module(const std::string& moduleName, bool enable_verbose=true); 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;
/// non-const function that performs parsing - typically called by constructor /// non-const function that performs parsing - typically called by constructor
/// Throws exception on failure /// Throws exception on failure
void parseMarkup(const std::string& data); 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: private:
static std::vector<Class> ExpandTypedefInstantiations(const std::vector<Class>& classes, const std::vector<TemplateInstantiationTypedef> instantiations); static std::vector<Class> ExpandTypedefInstantiations(
static std::vector<std::string> GenerateValidTypes(const std::vector<Class>& classes, const std::vector<ForwardDeclaration> forwardDeclarations); const std::vector<Class>& classes,
static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes); const std::vector<TemplateInstantiationTypedef> instantiations);
static void WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes); 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 } // \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 * @file ReturnValue.cpp
*
* @date Dec 1, 2011 * @date Dec 1, 2011
* @author Alex Cunningham * @author Alex Cunningham
* @author Andrew Melim * @author Andrew Melim
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <boost/foreach.hpp>
#include "ReturnValue.h" #include "ReturnValue.h"
#include "utilities.h" #include "utilities.h"
#include <boost/foreach.hpp>
#include <iostream>
using namespace std; using namespace std;
using namespace wrap; using namespace wrap;
/* ************************************************************************* */ /* ************************************************************************* */
string ReturnValue::return_type(bool add_ptr, pairing p) const { ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const {
if (p==pair && isPair) { ReturnValue instRetVal = *this;
string str = "pair< " + instRetVal.type1 = ts(type1);
maybe_shared_ptr(add_ptr || isPtr1, qualifiedType1("::"), type1) + ", " + if (isPair)
maybe_shared_ptr(add_ptr || isPtr2, qualifiedType2("::"), type2) + " >"; instRetVal.type2 = ts(type2);
return str; return instRetVal;
} else }
return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"), (p==arg2)? type2 : type1);
/* ************************************************************************* */
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 { string ReturnValue::matlab_returnType() const {
return isPair? "[first,second]" : "result"; return isPair ? "[first,second]" : "result";
} }
/* ************************************************************************* */ /* ************************************************************************* */
string ReturnValue::qualifiedType1(const string& delim) const { void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile,
string result; const TypeAttributesTable& typeAttributes) const {
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(".");
if (isPair) { if (isPair) {
// For a pair, store the returned pair so we do not evaluate the function twice // For a pair, store the returned pair so we do not evaluate the function twice
file.oss << " " << return_type(false, pair) << " pairResult = " << result << ";\n"; wrapperFile.oss << " " << return_type(true) << " pairResult = " << result
<< ";\n";
// first return value in pair type1.wrap_result(" out[0]", "pairResult.first", wrapperFile,
if (category1 == ReturnValue::CLASS) { // if we are going to make one typeAttributes);
string objCopy, ptrType; type2.wrap_result(" out[1]", "pairResult.second", wrapperFile,
ptrType = "Shared" + type1; typeAttributes);
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";
} else { // Not a pair } else { // Not a pair
type1.wrap_result(" out[0]", result, wrapperFile, typeAttributes);
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";
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const {
if(isPair) type1.wrapTypeUnwrap(wrapperFile);
{ if (isPair)
if(category1 == ReturnValue::CLASS) type2.wrapTypeUnwrap(wrapperFile);
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;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ReturnValue::emit_matlab(FileWriter& file) const { void ReturnValue::emit_matlab(FileWriter& proxyFile) const {
string output; string output;
if (isPair) if (isPair)
file.oss << "[ varargout{1} varargout{2} ] = "; proxyFile.oss << "[ varargout{1} varargout{2} ] = ";
else if (category1 != ReturnValue::VOID) else if (type1.category != ReturnType::VOID)
file.oss << "varargout{1} = "; proxyFile.oss << "varargout{1} = ";
} }
/* ************************************************************************* */ /* ************************************************************************* */

48
wrap/ReturnValue.h
View File

@ -2,59 +2,61 @@
* @file ReturnValue.h * @file ReturnValue.h
* *
* @brief Encapsulates a return value from a method * @brief Encapsulates a return value from a method
*
* @date Dec 1, 2011 * @date Dec 1, 2011
* @author Alex Cunningham * @author Alex Cunningham
* @author Richard Roberts * @author Richard Roberts
*/ */
#include "ReturnType.h"
#include "TemplateSubstitution.h"
#include "FileWriter.h" #include "FileWriter.h"
#include "TypeAttributesTable.h" #include "TypeAttributesTable.h"
#include "utilities.h"
#include <vector>
#pragma once #pragma once
namespace wrap { namespace wrap {
/** /**
* Encapsulates return value of a method or function * Encapsulates return type of a method or function, possibly a pair
*/ */
struct ReturnValue { struct ReturnValue {
/// the different supported return value categories bool isPair;
typedef enum { ReturnType type1, type2;
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;
/// Constructor /// Constructor
ReturnValue() : ReturnValue() :
isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2( isPair(false) {
CLASS) {
} }
typedef enum { /// Constructor
arg1, arg2, pair ReturnValue(const ReturnType& type) :
} pairing; 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 return_type(bool add_ptr) const;
std::string qualifiedType2(const std::string& delim = "") const;
std::string matlab_returnType() 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; const TypeAttributesTable& typeAttributes) const;
void wrapTypeUnwrap(FileWriter& wrapperFile) 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 } // \namespace wrap

170
wrap/StaticMethod.cpp
View File

@ -16,7 +16,7 @@
* @author Richard Roberts * @author Richard Roberts
**/ **/
#include "StaticMethod.h" #include "Method.h"
#include "utilities.h" #include "utilities.h"
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -30,117 +30,145 @@ using namespace std;
using namespace wrap; using namespace wrap;
/* ************************************************************************* */ /* ************************************************************************* */
void StaticMethod::addOverload(bool verbose, const std::string& name, void StaticMethod::proxy_header(FileWriter& proxyFile) const {
const ArgumentList& args, const ReturnValue& retVal) { string upperName = matlabName();
this->verbose = verbose; upperName[0] = toupper(upperName[0], locale());
this->name = name; proxyFile.oss << " function varargout = " << upperName << "(varargin)\n";
this->argLists.push_back(args);
this->returnVals.push_back(retVal);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void StaticMethod::proxy_wrapper_fragments(FileWriter& file, void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const string& cppClassName, FileWriter& wrapperFile, Str cppClassName, Str matlabQualName,
const std::string& matlabQualName, const std::string& matlabUniqueName, Str matlabUniqueName, Str wrapperName,
const string& wrapperName, const TypeAttributesTable& typeAttributes, const TypeAttributesTable& typeAttributes,
vector<string>& functionNames) const { vector<string>& functionNames) const {
string upperName = name; // emit header, e.g., function varargout = templatedMethod(this, varargin)
upperName[0] = std::toupper(upperName[0], std::locale()); proxy_header(proxyFile);
file.oss << " function varargout = " << upperName << "(varargin)\n"; // Emit comments for documentation
//Comments for documentation string up_name = boost::to_upper_copy(matlabName());
string up_name = boost::to_upper_copy(name); proxyFile.oss << " % " << up_name << " usage: ";
file.oss << " % " << up_name << " usage: "; usage_fragment(proxyFile, matlabName());
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++;
}
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" << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
<< endl; << 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 // 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(); const int id = (int) functionNames.size();
argLists[overload].emit_conditional_call(file, returnVals[overload], argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id,
wrapperName, id, true); // last bool is to indicate static ! isStatic());
// Output C++ wrapper code // Output C++ wrapper code
const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, 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);
} else {
// Check arguments for all overloads
for (size_t i = 0; i < nrOverloads(); ++i) {
// 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 // Add to function list
functionNames.push_back(wrapFunctionName); functionNames.push_back(wrapFunctionName);
} }
file.oss << " else\n"; proxyFile.oss << " else\n";
file.oss << " error('Arguments do not match any overload of function " proxyFile.oss
<< matlabQualName << "." << upperName << "');" << endl; << " error('Arguments do not match any overload of function "
file.oss << " end\n"; << matlabQualName << "." << name_ << "');" << endl;
proxyFile.oss << " end\n";
}
file.oss << " end\n"; proxyFile.oss << " end\n";
} }
/* ************************************************************************* */ /* ************************************************************************* */
string StaticMethod::wrapper_fragment(FileWriter& file, string StaticMethod::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
const string& cppClassName, const string& matlabUniqueName, int overload, Str matlabUniqueName, int overload, int id,
int id, const TypeAttributesTable& typeAttributes) const { const TypeAttributesTable& typeAttributes,
const Qualified& instName) const {
// generate code // generate code
const string wrapFunctionName = matlabUniqueName + "_" + name + "_" const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_"
+ boost::lexical_cast<string>(id); + boost::lexical_cast<string>(id);
const ArgumentList& args = argLists[overload]; const ArgumentList& args = argumentList(overload);
const ReturnValue& returnVal = returnVals[overload]; const ReturnValue& returnVal = returnValue(overload);
// call // call
file.oss << "void " << wrapFunctionName wrapperFile.oss << "void " << wrapFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
// start // start
file.oss << "{\n"; wrapperFile.oss << "{\n";
returnVal.wrapTypeUnwrap(file); returnVal.wrapTypeUnwrap(wrapperFile);
file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName
<< endl; << "> Shared;" << endl;
// check arguments // get call
// NOTE: for static functions, there is no object passed // for static methods: cppClassName::staticMethod<TemplateVal>
file.oss << " checkArguments(\"" << matlabUniqueName << "." << name // for instance methods: obj->instanceMethod<TemplateVal>
<< "\",nargout,nargin," << args.size() << ");\n"; string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName,
args, returnVal, typeAttributes, instName);
// unwrap arguments, see Argument.cpp expanded += ("(" + args.names() + ")");
args.matlab_unwrap(file, 0); // We start at 0 because there is no self object if (returnVal.type1.name != "void")
returnVal.wrap_result(expanded, wrapperFile, typeAttributes);
// call method with default type and wrap result
if (returnVal.type1 != "void")
returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")",
file, typeAttributes);
else else
file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n"; wrapperFile.oss << " " + expanded + ";\n";
// finish // finish
file.oss << "}\n"; wrapperFile.oss << "}\n";
return wrapFunctionName; 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 #pragma once
#include "Argument.h" #include "FullyOverloadedFunction.h"
#include "ReturnValue.h"
#include "TypeAttributesTable.h"
namespace wrap { namespace wrap {
/// StaticMethod class /// StaticMethod class
struct StaticMethod { struct StaticMethod: public FullyOverloadedFunction {
/// Constructor creates empty object typedef const std::string& Str;
StaticMethod(bool verbosity = true) :
verbose(verbosity) { virtual bool isStatic() const {
return true;
} }
// Then the instance variables are set directly by the Module constructor // emit a list of comments, one for each overload
bool verbose; void comment_fragment(FileWriter& proxyFile) const {
std::string name; SignatureOverloads::comment_fragment(proxyFile, matlabName());
std::vector<ArgumentList> argLists; }
std::vector<ReturnValue> returnVals;
// The first time this function is called, it initializes the class members void verifyArguments(const std::vector<std::string>& validArgs) const {
// with those in rhs, but in subsequent calls it adds additional argument SignatureOverloads::verifyArguments(validArgs, name_);
// lists as function overloads. }
void addOverload(bool verbose, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal); void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
SignatureOverloads::verifyReturnTypes(validtypes, name_);
}
// MATLAB code generation // MATLAB code generation
// classPath is class directory, e.g., ../matlab/@Point2 // classPath is class directory, e.g., ../matlab/@Point2
void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& cppClassName, const std::string& matlabQualName, Str cppClassName, Str matlabQualName, Str matlabUniqueName,
const std::string& matlabUniqueName, const std::string& wrapperName, Str wrapperName, const TypeAttributesTable& typeAttributes,
const TypeAttributesTable& typeAttributes,
std::vector<std::string>& functionNames) const; std::vector<std::string>& functionNames) const;
private: // emit python wrapper
std::string wrapper_fragment(FileWriter& file, void python_wrapper(FileWriter& wrapperFile, Str className) const;
const std::string& cppClassName, const std::string& matlabUniqueName,
int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper 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 } // \namespace wrap

43
wrap/TemplateInstantiationTypedef.cpp
View File

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

9
wrap/TemplateInstantiationTypedef.h
View File

@ -26,12 +26,9 @@
namespace wrap { namespace wrap {
struct TemplateInstantiationTypedef { struct TemplateInstantiationTypedef : public Qualified {
std::vector<std::string> classNamespaces; Qualified class_;
std::string className; std::vector<Qualified> typeList;
std::vector<std::string> namespaces;
std::string name;
std::vector<std::vector<std::string> > typeList;
Class findAndExpand(const std::vector<Class>& classes) const; 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

14
wrap/TypeAttributesTable.cpp
View File

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

2
wrap/TypeAttributesTable.h
View File

@ -28,7 +28,7 @@
namespace wrap { namespace wrap {
// Forward declarations // Forward declarations
struct Class; class Class;
/** Attributes about valid classes, both for classes defined in this module and /** Attributes about valid classes, both for classes defined in this module and
* also those forward-declared from others. At the moment this only contains * also those forward-declared from others. At the moment this only contains

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