diff --git a/.cproject b/.cproject
index 62b32938c..412359938 100644
--- a/.cproject
+++ b/.cproject
@@ -2353,6 +2353,38 @@
true
true
+
+ make
+ -j5
+ testSpirit.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.wrap
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testMethod.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testClass.run
+ true
+ true
+ true
+
make
-j5
@@ -3222,22 +3254,6 @@
true
true
-
- make
- -j5
- testSpirit.run
- true
- true
- true
-
-
- make
- -j5
- check.wrap
- true
- true
- true
-
make
-j5
diff --git a/gtsam.h b/gtsam.h
index 678e2a3d6..40d204a5f 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -156,8 +156,14 @@ virtual class Value {
size_t dim() const;
};
+class Vector3 {
+ Vector3(Vector v);
+};
+class Vector6 {
+ Vector6(Vector v);
+};
#include
-virtual class LieScalar : gtsam::Value {
+class LieScalar {
// Standard constructors
LieScalar();
LieScalar(double d);
@@ -186,7 +192,7 @@ virtual class LieScalar : gtsam::Value {
};
#include
-virtual class LieVector : gtsam::Value {
+class LieVector {
// Standard constructors
LieVector();
LieVector(Vector v);
@@ -218,7 +224,7 @@ virtual class LieVector : gtsam::Value {
};
#include
-virtual class LieMatrix : gtsam::Value {
+class LieMatrix {
// Standard constructors
LieMatrix();
LieMatrix(Matrix v);
@@ -253,7 +259,7 @@ virtual class LieMatrix : gtsam::Value {
// geometry
//*************************************************************************
-virtual class Point2 : gtsam::Value {
+class Point2 {
// Standard Constructors
Point2();
Point2(double x, double y);
@@ -290,7 +296,7 @@ virtual class Point2 : gtsam::Value {
void serialize() const;
};
-virtual class StereoPoint2 : gtsam::Value {
+class StereoPoint2 {
// Standard Constructors
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
@@ -325,7 +331,7 @@ virtual class StereoPoint2 : gtsam::Value {
void serialize() const;
};
-virtual class Point3 : gtsam::Value {
+class Point3 {
// Standard Constructors
Point3();
Point3(double x, double y, double z);
@@ -361,7 +367,7 @@ virtual class Point3 : gtsam::Value {
void serialize() const;
};
-virtual class Rot2 : gtsam::Value {
+class Rot2 {
// Standard Constructors and Named Constructors
Rot2();
Rot2(double theta);
@@ -406,7 +412,7 @@ virtual class Rot2 : gtsam::Value {
void serialize() const;
};
-virtual class Rot3 : gtsam::Value {
+class Rot3 {
// Standard Constructors and Named Constructors
Rot3();
Rot3(Matrix R);
@@ -462,7 +468,7 @@ virtual class Rot3 : gtsam::Value {
void serialize() const;
};
-virtual class Pose2 : gtsam::Value {
+class Pose2 {
// Standard Constructor
Pose2();
Pose2(const gtsam::Pose2& pose);
@@ -512,7 +518,7 @@ virtual class Pose2 : gtsam::Value {
void serialize() const;
};
-virtual class Pose3 : gtsam::Value {
+class Pose3 {
// Standard Constructors
Pose3();
Pose3(const gtsam::Pose3& pose);
@@ -564,7 +570,7 @@ virtual class Pose3 : gtsam::Value {
};
#include
-virtual class Unit3 : gtsam::Value {
+class Unit3 {
// Standard Constructors
Unit3();
Unit3(const gtsam::Point3& pose);
@@ -585,7 +591,7 @@ virtual class Unit3 : gtsam::Value {
};
#include
-virtual class EssentialMatrix : gtsam::Value {
+class EssentialMatrix {
// Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
@@ -606,7 +612,7 @@ virtual class EssentialMatrix : gtsam::Value {
double error(Vector vA, Vector vB);
};
-virtual class Cal3_S2 : gtsam::Value {
+class Cal3_S2 {
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
@@ -643,7 +649,7 @@ virtual class Cal3_S2 : gtsam::Value {
};
#include
-virtual class Cal3DS2 : gtsam::Value {
+class Cal3DS2 {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
@@ -699,7 +705,43 @@ class Cal3_S2Stereo {
double baseline() const;
};
-virtual class CalibratedCamera : gtsam::Value {
+#include
+class Cal3Bundler {
+ // Standard Constructors
+ Cal3Bundler();
+ Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+
+ // Testable
+ void print(string s) const;
+ bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
+
+ // Manifold
+ static size_t Dim();
+ size_t dim() const;
+ gtsam::Cal3Bundler retract(Vector v) const;
+ Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
+
+ // Action on Point2
+ gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+ gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+ gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+ // Standard Interface
+ double fx() const;
+ double fy() const;
+ double k1() const;
+ double k2() const;
+ double u0() const;
+ double v0() const;
+ Vector vector() const;
+ Vector k() const;
+ //Matrix K() const; //FIXME: Uppercase
+
+ // enabling serialization functionality
+ void serialize() const;
+};
+
+class CalibratedCamera {
// Standard Constructors and Named Constructors
CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose);
@@ -732,7 +774,7 @@ virtual class CalibratedCamera : gtsam::Value {
void serialize() const;
};
-virtual class SimpleCamera : gtsam::Value {
+class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
@@ -771,7 +813,7 @@ virtual class SimpleCamera : gtsam::Value {
};
template
-virtual class PinholeCamera : gtsam::Value {
+class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::Pose3& pose);
@@ -809,7 +851,7 @@ virtual class PinholeCamera : gtsam::Value {
void serialize() const;
};
-virtual class StereoCamera : gtsam::Value {
+class StereoCamera {
// Standard Constructors and Named Constructors
StereoCamera();
StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
@@ -862,7 +904,7 @@ virtual class SymbolicFactor {
};
#include
-class SymbolicFactorGraph {
+virtual class SymbolicFactorGraph {
SymbolicFactorGraph();
SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
@@ -1664,15 +1706,12 @@ class Values {
void print(string s) const;
bool equals(const gtsam::Values& other, double tol) const;
- void insert(size_t j, const gtsam::Value& value);
void insert(const gtsam::Values& values);
- void update(size_t j, const gtsam::Value& val);
void update(const gtsam::Values& values);
void erase(size_t j);
void swap(gtsam::Values& values);
bool exists(size_t j) const;
- gtsam::Value at(size_t j) const;
gtsam::KeyList keys() const;
gtsam::VectorValues zeroVectors() const;
@@ -1682,6 +1721,37 @@ class Values {
// enabling serialization functionality
void serialize() const;
+
+ // New in 4.0, we have to specialize every insert/update/at to generate wrappers
+ // Instead of the old:
+ // void insert(size_t j, const gtsam::Value& value);
+ // void update(size_t j, const gtsam::Value& val);
+ // gtsam::Value at(size_t j) const;
+ void insert(size_t j, const gtsam::Point2& t);
+ void insert(size_t j, const gtsam::Point3& t);
+ void insert(size_t j, const gtsam::Rot2& t);
+ void insert(size_t j, const gtsam::Pose2& t);
+ void insert(size_t j, const gtsam::Rot3& t);
+ void insert(size_t j, const gtsam::Pose3& t);
+ void insert(size_t j, const gtsam::Cal3_S2& t);
+ void insert(size_t j, const gtsam::Cal3DS2& t);
+ void insert(size_t j, const gtsam::Cal3Bundler& t);
+ void insert(size_t j, const gtsam::EssentialMatrix& t);
+
+ void update(size_t j, const gtsam::Point2& t);
+ void update(size_t j, const gtsam::Point3& t);
+ void update(size_t j, const gtsam::Rot2& t);
+ void update(size_t j, const gtsam::Pose2& t);
+ void update(size_t j, const gtsam::Rot3& t);
+ void update(size_t j, const gtsam::Pose3& t);
+ void update(size_t j, const gtsam::Cal3_S2& t);
+ void update(size_t j, const gtsam::Cal3DS2& t);
+ void update(size_t j, const gtsam::Cal3Bundler& t);
+ void update(size_t j, const gtsam::EssentialMatrix& t);
+
+ template
+ T at(size_t j);
};
// Actually a FastList
@@ -2077,7 +2147,7 @@ class NonlinearISAM {
#include
#include
-template
+template
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@@ -2088,7 +2158,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include
-template
+template
virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const;
@@ -2099,7 +2169,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include
-template
+template
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible);
@@ -2280,7 +2350,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph,
namespace imuBias {
#include
-virtual class ConstantBias : gtsam::Value {
+class ConstantBias {
// Standard Constructor
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
@@ -2340,7 +2410,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
- void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+ void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
@@ -2383,7 +2453,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
- void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+ void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 39f4b7996..95524e115 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -37,7 +37,7 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
-class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue {
+class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
typedef Cal3DS2_Base Base;
@@ -87,21 +87,24 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable
- /// @}
private:
+ /// @}
+ /// @name Advanced Interface
+ /// @{
+
/** Serialization function */
friend class boost::serialization::access;
template
void serialize(Archive & ar, const unsigned int version)
{
- ar & boost::serialization::make_nvp("Cal3DS2",
- boost::serialization::base_object(*this));
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object(*this));
}
+ /// @}
+
};
// Define GTSAM traits
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index b8181ab4d..e4397a449 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
}
/* ************************************************************************* */
-static Eigen::Matrix D2dcalibration(double x, double y, double xx,
+static Matrix29 D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
- const Eigen::Matrix& DK) {
- Eigen::Matrix DR1;
+ const Matrix2& DK) {
+ Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
- Eigen::Matrix DR2;
+ Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
- Eigen::Matrix D;
+ Matrix29 D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
-static Eigen::Matrix D2dintrinsic(double x, double y, double rr,
+static Matrix2 D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
- const Eigen::Matrix& DK) {
+ const Matrix2& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@@ -82,7 +82,7 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr,
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
- Eigen::Matrix DR;
+ Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
@@ -90,8 +90,9 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr,
}
/* ************************************************************************* */
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1,
- boost::optional H2) const {
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
+ boost::optional H1,
+ boost::optional H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
@@ -110,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1,
const double pnx = g * x + dx;
const double pny = g * y + dy;
- Eigen::Matrix DK;
+ Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
@@ -125,6 +126,26 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1,
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
+/* ************************************************************************* */
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
+ boost::optional H1,
+ boost::optional H2) const {
+
+ if (H1 || H2) {
+ Matrix29 D1;
+ Matrix2 D2;
+ Point2 pu = uncalibrate(p, D1, D2);
+ if (H1)
+ *H1 = D1;
+ if (H2)
+ *H2 = D2;
+ return pu;
+
+ } else {
+ return uncalibrate(p);
+ }
+}
+
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
@@ -161,7 +182,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
- Eigen::Matrix DK;
+ Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
@@ -176,7 +197,7 @@ Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
- Eigen::Matrix DK;
+ Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index 7be5a6874..61c01e349 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -18,7 +18,6 @@
#pragma once
-#include
#include
namespace gtsam {
@@ -114,9 +113,14 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
+
Point2 uncalibrate(const Point2& p,
- boost::optional Dcal = boost::none,
- boost::optional Dp = boost::none) const ;
+ boost::optional Dcal = boost::none,
+ boost::optional Dp = boost::none) const ;
+
+ Point2 uncalibrate(const Point2& p,
+ boost::optional Dcal,
+ boost::optional Dp) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
@@ -127,7 +131,6 @@ public:
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
-
private:
/// @}
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index e7b408982..6113714a1 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -50,6 +50,7 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
}
/* ************************************************************************* */
+// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
boost::optional H1,
boost::optional H2) const {
@@ -70,26 +71,30 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
// Part2: project NPlane point to pixel plane: use Cal3DS2
Point2 m(x,y);
- Matrix H1base, H2base; // jacobians from Base class
+ Matrix29 H1base;
+ Matrix2 H2base; // jacobians from Base class
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
// Inlined derivative for calibration
if (H1) {
// part1
- Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2,
- -ys * sqrt_nx * xi_sqrt_nx2);
- Matrix DDS2U = H2base * DU;
+ Vector2 DU;
+ DU << -xs * sqrt_nx * xi_sqrt_nx2, //
+ -ys * sqrt_nx * xi_sqrt_nx2;
- *H1 = collect(2, &H1base, &DDS2U);
+ H1->resize(2,10);
+ H1->block<2,9>(0,0) = H1base;
+ H1->block<2,1>(0,9) = H2base * DU;
}
+
// Inlined derivative for points
if (H2) {
// part1
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
const double mid = -(xi * xs*ys) * denom;
- Matrix DU = (Matrix(2, 2) <<
- (sqrt_nx + xi*(ys*ys + 1)) * denom, mid,
- mid, (sqrt_nx + xi*(xs*xs + 1)) * denom);
+ Matrix2 DU;
+ DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
+ mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
*H2 = H2base * DU;
}
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index dc167173e..fb99592f7 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -40,7 +40,7 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
-class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue {
+class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
typedef Cal3Unified This;
typedef Cal3DS2_Base Base;
@@ -129,8 +129,6 @@ private:
template
void serialize(Archive & ar, const unsigned int version)
{
- ar & boost::serialization::make_nvp("Cal3Unified",
- boost::serialization::base_object(*this));
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index e23b22093..8763504c0 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -58,6 +58,8 @@ public:
return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
}
+ virtual ~EssentialMatrix() {}
+
/// @}
/// @name Testable
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index 27460bd72..95e635516 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -4,14 +4,15 @@
// specify the classes from gtsam we are using
virtual class gtsam::Value;
-virtual class gtsam::LieScalar;
-virtual class gtsam::LieVector;
-virtual class gtsam::Point2;
-virtual class gtsam::Rot2;
-virtual class gtsam::Pose2;
-virtual class gtsam::Point3;
-virtual class gtsam::Rot3;
-virtual class gtsam::Pose3;
+class gtsam::Vector6;
+class gtsam::LieScalar;
+class gtsam::LieVector;
+class gtsam::Point2;
+class gtsam::Rot2;
+class gtsam::Pose2;
+class gtsam::Point3;
+class gtsam::Rot3;
+class gtsam::Pose3;
virtual class gtsam::noiseModel::Base;
virtual class gtsam::noiseModel::Gaussian;
virtual class gtsam::imuBias::ConstantBias;
@@ -23,8 +24,8 @@ virtual class gtsam::NoiseModelFactor4;
virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor;
-virtual class gtsam::Cal3_S2;
-virtual class gtsam::Cal3DS2;
+class gtsam::Cal3_S2;
+class gtsam::Cal3DS2;
class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph;
class gtsam::Ordering;
@@ -48,7 +49,7 @@ class Dummy {
};
#include
-virtual class PoseRTV : gtsam::Value {
+class PoseRTV {
PoseRTV();
PoseRTV(Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
@@ -99,7 +100,7 @@ virtual class PoseRTV : gtsam::Value {
};
#include
-virtual class Pose3Upright : gtsam::Value {
+class Pose3Upright {
Pose3Upright();
Pose3Upright(const gtsam::Pose3Upright& x);
Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
@@ -137,7 +138,7 @@ virtual class Pose3Upright : gtsam::Value {
}; // \class Pose3Upright
#include
-virtual class BearingS2 : gtsam::Value {
+class BearingS2 {
BearingS2();
BearingS2(double azimuth, double elevation);
BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);
@@ -520,14 +521,14 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
virtual class Reconstruction : gtsam::NonlinearFactor {
Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h);
- Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::LieVector& xiK) const;
+ Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::Vector6& xiK) const;
};
virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor {
DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey,
double h, Matrix Inertia, Vector Fu, double m);
- Vector evaluateError(const gtsam::LieVector& xiK, const gtsam::LieVector& xiK_1, const gtsam::Pose3& gK) const;
+ Vector evaluateError(const gtsam::Vector6& xiK, const gtsam::Vector6& xiK_1, const gtsam::Pose3& gK) const;
};
//*************************************************************************
diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h
index c147552b3..9082c0101 100644
--- a/gtsam_unstable/slam/BetweenFactorEM.h
+++ b/gtsam_unstable/slam/BetweenFactorEM.h
@@ -25,384 +25,405 @@
namespace gtsam {
+/**
+ * A class for a measurement predicted by "between(config[key1],config[key2])"
+ * @tparam VALUE the Value type
+ * @addtogroup SLAM
+ */
+template
+class BetweenFactorEM: public NonlinearFactor {
+
+public:
+
+ typedef VALUE T;
+
+private:
+
+ typedef BetweenFactorEM This;
+ typedef gtsam::NonlinearFactor Base;
+
+ gtsam::Key key1_;
+ gtsam::Key key2_;
+
+ VALUE measured_; /** The measurement */
+
+ SharedGaussian model_inlier_;
+ SharedGaussian model_outlier_;
+
+ double prior_inlier_;
+ double prior_outlier_;
+
+ bool flag_bump_up_near_zero_probs_;
+
+ /** concept check by type */
+ GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T)
+
+public:
+
+ // shorthand for a smart pointer to a factor
+ typedef typename boost::shared_ptr shared_ptr;
+
+ /** default constructor - only use for serialization */
+ BetweenFactorEM() {
+ }
+
+ /** Constructor */
+ BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
+ const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
+ const double prior_inlier, const double prior_outlier,
+ const bool flag_bump_up_near_zero_probs = false) :
+ Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(
+ measured), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_(
+ prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(
+ flag_bump_up_near_zero_probs) {
+ }
+
+ virtual ~BetweenFactorEM() {
+ }
+
+ /** implement functions needed for Testable */
+
+ /** print */
+ virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+ DefaultKeyFormatter) const {
+ std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << ","
+ << keyFormatter(key2_) << ")\n";
+ measured_.print(" measured: ");
+ model_inlier_->print(" noise model inlier: ");
+ model_outlier_->print(" noise model outlier: ");
+ std::cout << "(prior_inlier, prior_outlier_) = (" << prior_inlier_ << ","
+ << prior_outlier_ << ")\n";
+ // Base::print(s, keyFormatter);
+ }
+
+ /** equals */
+ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+ const This *t = dynamic_cast(&f);
+
+ if (t && Base::equals(f))
+ return key1_ == t->key1_ && key2_ == t->key2_
+ &&
+ // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
+ // model_outlier_->equals(t->model_outlier_ ) &&
+ prior_outlier_ == t->prior_outlier_
+ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
+ else
+ return false;
+ }
+
+ /** implement functions needed to derive from Factor */
+
+ /* ************************************************************************* */
+ virtual double error(const gtsam::Values& x) const {
+ return whitenedError(x).squaredNorm();
+ }
+
+ /* ************************************************************************* */
/**
- * A class for a measurement predicted by "between(config[key1],config[key2])"
- * @tparam VALUE the Value type
- * @addtogroup SLAM
+ * Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
+ * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
+ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
- template
- class BetweenFactorEM: public NonlinearFactor {
+ /* This version of linearize recalculates the noise model each time */
+ virtual boost::shared_ptr linearize(
+ const gtsam::Values& x) const {
+ // Only linearize if the factor is active
+ if (!this->active(x))
+ return boost::shared_ptr();
- public:
+ //std::cout<<"About to linearize"< A(this->size());
+ gtsam::Vector b = -whitenedError(x, A);
+ A1 = A[0];
+ A2 = A[1];
- typedef VALUE T;
+ return gtsam::GaussianFactor::shared_ptr(
+ new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
+ gtsam::noiseModel::Unit::Create(b.size())));
+ }
- private:
+ /* ************************************************************************* */
+ gtsam::Vector whitenedError(const gtsam::Values& x,
+ boost::optional&> H = boost::none) const {
- typedef BetweenFactorEM This;
- typedef gtsam::NonlinearFactor Base;
+ bool debug = true;
- gtsam::Key key1_;
- gtsam::Key key2_;
+ const T& p1 = x.at(key1_);
+ const T& p2 = x.at(key2_);
- VALUE measured_; /** The measurement */
+ Matrix H1, H2;
- SharedGaussian model_inlier_;
- SharedGaussian model_outlier_;
+ T hx = p1.between(p2, H1, H2); // h(x)
+ // manifold equivalent of h(x)-z -> log(z,h(x))
- double prior_inlier_;
- double prior_outlier_;
+ Vector err = measured_.localCoordinates(hx);
- bool flag_bump_up_near_zero_probs_;
+ // Calculate indicator probabilities (inlier and outlier)
+ Vector p_inlier_outlier = calcIndicatorProb(x);
+ double p_inlier = p_inlier_outlier[0];
+ double p_outlier = p_inlier_outlier[1];
- /** concept check by type */
- GTSAM_CONCEPT_LIE_TYPE(T)
- GTSAM_CONCEPT_TESTABLE_TYPE(T)
+ Vector err_wh_inlier = model_inlier_->whiten(err);
+ Vector err_wh_outlier = model_outlier_->whiten(err);
- public:
+ Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
+ Matrix invCov_outlier = model_outlier_->R().transpose()
+ * model_outlier_->R();
- // shorthand for a smart pointer to a factor
- typedef typename boost::shared_ptr shared_ptr;
+ Vector err_wh_eq;
+ err_wh_eq.resize(err_wh_inlier.rows() * 2);
+ err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier)
+ * err_wh_outlier.array();
- /** default constructor - only use for serialization */
- BetweenFactorEM() {}
+ if (H) {
+ // stack Jacobians for the two indicators for each of the key
- /** Constructor */
- BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
- const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
- const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false) :
- Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured),
- model_inlier_(model_inlier), model_outlier_(model_outlier),
- prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs){
+ Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
+ Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
+ Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
+
+ Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
+ Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
+ Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
+
+ (*H)[0].resize(H1_aug.rows(), H1_aug.cols());
+ (*H)[1].resize(H2_aug.rows(), H2_aug.cols());
+
+ (*H)[0] = H1_aug;
+ (*H)[1] = H2_aug;
}
- virtual ~BetweenFactorEM() {}
+ if (debug) {
+ // std::cout<<"unwhitened error: "<print(" noise model inlier: ");
- model_outlier_->print(" noise model outlier: ");
- std::cout << "(prior_inlier, prior_outlier_) = ("
- << prior_inlier_ << ","
- << prior_outlier_ << ")\n";
- // Base::print(s, keyFormatter);
+ // Matrix Cov_inlier = invCov_inlier.inverse();
+ // Matrix Cov_outlier = invCov_outlier.inverse();
+ // std::cout<<"Cov_inlier: "< (&f);
+ return err_wh_eq;
+ }
- if(t && Base::equals(f))
- return key1_ == t->key1_ && key2_ == t->key2_ &&
- // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
- // model_outlier_->equals(t->model_outlier_ ) &&
- prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
- else
- return false;
+ /* ************************************************************************* */
+ gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
+
+ bool debug = false;
+
+ Vector err = unwhitenedError(x);
+
+ // Calculate indicator probabilities (inlier and outlier)
+ Vector err_wh_inlier = model_inlier_->whiten(err);
+ Vector err_wh_outlier = model_outlier_->whiten(err);
+
+ Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
+ Matrix invCov_outlier = model_outlier_->R().transpose()
+ * model_outlier_->R();
+
+ double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant())
+ * exp(-0.5 * err_wh_inlier.dot(err_wh_inlier));
+ double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant())
+ * exp(-0.5 * err_wh_outlier.dot(err_wh_outlier));
+
+ if (debug) {
+ std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", "
+ << err[1] << ", " << err[2] << std::endl;
+ std::cout << "in calcIndicatorProb. err_wh_inlier: " << err_wh_inlier[0]
+ << ", " << err_wh_inlier[1] << ", " << err_wh_inlier[2] << std::endl;
+ std::cout << "in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "
+ << err_wh_inlier.dot(err_wh_inlier) << std::endl;
+ std::cout << "in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "
+ << err_wh_outlier.dot(err_wh_outlier) << std::endl;
+
+ std::cout
+ << "in calcIndicatorProb. p_inlier, p_outlier before normalization: "
+ << p_inlier << ", " << p_outlier << std::endl;
}
- /** implement functions needed to derive from Factor */
+ double sumP = p_inlier + p_outlier;
+ p_inlier /= sumP;
+ p_outlier /= sumP;
- /* ************************************************************************* */
- virtual double error(const gtsam::Values& x) const {
- return whitenedError(x).squaredNorm();
+ if (flag_bump_up_near_zero_probs_) {
+ // Bump up near-zero probabilities (as in linerFlow.h)
+ double minP = 0.05; // == 0.1 / 2 indicator variables
+ if (p_inlier < minP || p_outlier < minP) {
+ if (p_inlier < minP)
+ p_inlier = minP;
+ if (p_outlier < minP)
+ p_outlier = minP;
+ sumP = p_inlier + p_outlier;
+ p_inlier /= sumP;
+ p_outlier /= sumP;
+ }
}
- /* ************************************************************************* */
- /**
- * Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
- * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
- * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
+ return (Vector(2) << p_inlier, p_outlier);
+ }
+
+ /* ************************************************************************* */
+ gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
+
+ const T& p1 = x.at(key1_);
+ const T& p2 = x.at(key2_);
+
+ Matrix H1, H2;
+
+ T hx = p1.between(p2, H1, H2); // h(x)
+
+ return measured_.localCoordinates(hx);
+ }
+
+ /* ************************************************************************* */
+ void set_flag_bump_up_near_zero_probs(bool flag) {
+ flag_bump_up_near_zero_probs_ = flag;
+ }
+
+ /* ************************************************************************* */
+ bool get_flag_bump_up_near_zero_probs() const {
+ return flag_bump_up_near_zero_probs_;
+ }
+
+ /* ************************************************************************* */
+ SharedGaussian get_model_inlier() const {
+ return model_inlier_;
+ }
+
+ /* ************************************************************************* */
+ SharedGaussian get_model_outlier() const {
+ return model_outlier_;
+ }
+
+ /* ************************************************************************* */
+ Matrix get_model_inlier_cov() const {
+ return (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
+ }
+
+ /* ************************************************************************* */
+ Matrix get_model_outlier_cov() const {
+ return (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
+ }
+
+ /* ************************************************************************* */
+ void updateNoiseModels(const gtsam::Values& values,
+ const gtsam::NonlinearFactorGraph& graph) {
+ /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
+ * (note these are given in the E step, where indicator probabilities are calculated).
+ *
+ * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
+ * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
+ *
+ * TODO: improve efficiency (info form)
*/
- /* This version of linearize recalculates the noise model each time */
- virtual boost::shared_ptr linearize(const gtsam::Values& x) const {
- // Only linearize if the factor is active
- if (!this->active(x))
- return boost::shared_ptr();
- //std::cout<<"About to linearize"< A(this->size());
- gtsam::Vector b = -whitenedError(x, A);
- A1 = A[0];
- A2 = A[1];
+ // get joint covariance of the involved states
+ std::vector Keys;
+ Keys.push_back(key1_);
+ Keys.push_back(key2_);
+ Marginals marginals(graph, values, Marginals::QR);
+ JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
+ Matrix cov1 = joint_marginal12(key1_, key1_);
+ Matrix cov2 = joint_marginal12(key2_, key2_);
+ Matrix cov12 = joint_marginal12(key1_, key2_);
- return gtsam::GaussianFactor::shared_ptr(
- new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size())));
- }
+ updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
+ }
+ /* ************************************************************************* */
+ void updateNoiseModels_givenCovs(const gtsam::Values& values,
+ const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
+ /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
+ * (note these are given in the E step, where indicator probabilities are calculated).
+ *
+ * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
+ * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
+ *
+ * TODO: improve efficiency (info form)
+ */
- /* ************************************************************************* */
- gtsam::Vector whitenedError(const gtsam::Values& x,
- boost::optional&> H = boost::none) const {
+ const T& p1 = values.at(key1_);
+ const T& p2 = values.at(key2_);
- bool debug = true;
+ Matrix H1, H2;
+ p1.between(p2, H1, H2); // h(x)
- const T& p1 = x.at(key1_);
- const T& p2 = x.at(key2_);
+ Matrix H;
+ H.resize(H1.rows(), H1.rows() + H2.rows());
+ H << H1, H2; // H = [H1 H2]
- Matrix H1, H2;
+ Matrix joint_cov;
+ joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols());
+ joint_cov << cov1, cov12, cov12.transpose(), cov2;
- T hx = p1.between(p2, H1, H2); // h(x)
- // manifold equivalent of h(x)-z -> log(z,h(x))
+ Matrix cov_state = H * joint_cov * H.transpose();
- Vector err = measured_.localCoordinates(hx);
+ // model_inlier_->print("before:");
- // Calculate indicator probabilities (inlier and outlier)
- Vector p_inlier_outlier = calcIndicatorProb(x);
- double p_inlier = p_inlier_outlier[0];
- double p_outlier = p_inlier_outlier[1];
+ // update inlier and outlier noise models
+ Matrix covRinlier =
+ (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
+ model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(
+ covRinlier + cov_state);
- Vector err_wh_inlier = model_inlier_->whiten(err);
- Vector err_wh_outlier = model_outlier_->whiten(err);
+ Matrix covRoutlier =
+ (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
+ model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(
+ covRoutlier + cov_state);
- Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
- Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
+ // model_inlier_->print("after:");
+ // std::cout<<"covRinlier + cov_state: "<Whiten(H1);
- Matrix H1_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H1);
- Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
+ virtual size_t dim() const {
+ return model_inlier_->R().rows() + model_inlier_->R().cols();
+ }
- Matrix H2_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H2);
- Matrix H2_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H2);
- Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
+private:
- (*H)[0].resize(H1_aug.rows(),H1_aug.cols());
- (*H)[1].resize(H2_aug.rows(),H2_aug.cols());
+ /** Serialization function */
+ friend class boost::serialization::access;
+ template
+ void serialize(ARCHIVE & ar, const unsigned int version) {
+ ar
+ & boost::serialization::make_nvp("NonlinearFactor",
+ boost::serialization::base_object(*this));
+ ar & BOOST_SERIALIZATION_NVP(measured_);
+ }
+};
+// \class BetweenFactorEM
- (*H)[0] = H1_aug;
- (*H)[1] = H2_aug;
- }
-
- if (debug){
- // std::cout<<"unwhitened error: "<whiten(err);
- Vector err_wh_outlier = model_outlier_->whiten(err);
-
- Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
- Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
-
- double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
- double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
-
- if (debug){
- std::cout<<"in calcIndicatorProb. err_unwh: "<(key1_);
- const T& p2 = x.at(key2_);
-
- Matrix H1, H2;
-
- T hx = p1.between(p2, H1, H2); // h(x)
-
- return measured_.localCoordinates(hx);
- }
-
- /* ************************************************************************* */
- void set_flag_bump_up_near_zero_probs(bool flag) {
- flag_bump_up_near_zero_probs_ = flag;
- }
-
- /* ************************************************************************* */
- bool get_flag_bump_up_near_zero_probs() const {
- return flag_bump_up_near_zero_probs_;
- }
-
- /* ************************************************************************* */
- SharedGaussian get_model_inlier() const {
- return model_inlier_;
- }
-
- /* ************************************************************************* */
- SharedGaussian get_model_outlier() const {
- return model_outlier_;
- }
-
- /* ************************************************************************* */
- Matrix get_model_inlier_cov() const {
- return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
- }
-
- /* ************************************************************************* */
- Matrix get_model_outlier_cov() const {
- return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
- }
-
- /* ************************************************************************* */
- void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
- /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
- * (note these are given in the E step, where indicator probabilities are calculated).
- *
- * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
- * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
- *
- * TODO: improve efficiency (info form)
- */
-
- // get joint covariance of the involved states
- std::vector Keys;
- Keys.push_back(key1_);
- Keys.push_back(key2_);
- Marginals marginals( graph, values, Marginals::QR );
- JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
- Matrix cov1 = joint_marginal12(key1_, key1_);
- Matrix cov2 = joint_marginal12(key2_, key2_);
- Matrix cov12 = joint_marginal12(key1_, key2_);
-
- updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
- }
-
- /* ************************************************************************* */
- void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
- /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
- * (note these are given in the E step, where indicator probabilities are calculated).
- *
- * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
- * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
- *
- * TODO: improve efficiency (info form)
- */
-
- const T& p1 = values.at(key1_);
- const T& p2 = values.at(key2_);
-
- Matrix H1, H2;
- T hx = p1.between(p2, H1, H2); // h(x)
-
- Matrix H;
- H.resize(H1.rows(), H1.rows()+H2.rows());
- H << H1, H2; // H = [H1 H2]
-
- Matrix joint_cov;
- joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
- joint_cov << cov1, cov12,
- cov12.transpose(), cov2;
-
- Matrix cov_state = H*joint_cov*H.transpose();
-
- // model_inlier_->print("before:");
-
- // update inlier and outlier noise models
- Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
- model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
-
- Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
- model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
-
- // model_inlier_->print("after:");
- // std::cout<<"covRinlier + cov_state: "<R().rows() + model_inlier_->R().cols();
- }
-
- private:
-
- /** Serialization function */
- friend class boost::serialization::access;
- template
- void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("NonlinearFactor",
- boost::serialization::base_object(*this));
- ar & BOOST_SERIALIZATION_NVP(measured_);
- }
- }; // \class BetweenFactorEM
-
-} /// namespace gtsam
+}/// namespace gtsam
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
index 35010cdc6..2f0b2c7a8 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
@@ -372,7 +372,7 @@ namespace gtsam {
const T& p2 = values.at(keyB_);
Matrix H1, H2;
- T hx = p1.between(p2, H1, H2); // h(x)
+ p1.between(p2, H1, H2); // h(x)
Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows());
diff --git a/matlab/+gtsam/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m
index 874dbf523..9b52f016f 100644
--- a/matlab/+gtsam/VisualISAMPlot.m
+++ b/matlab/+gtsam/VisualISAMPlot.m
@@ -16,7 +16,7 @@ gtsam.plot3DPoints(result, [], marginals);
M = 1;
while result.exists(symbol('x',M))
ii = symbol('x',M);
- pose_i = result.at(ii);
+ pose_i = result.atPose3(ii);
if options.hardConstraint && (M==1)
gtsam.plotPose3(pose_i,[],10);
else
diff --git a/matlab/+gtsam/VisualISAMStep.m b/matlab/+gtsam/VisualISAMStep.m
index 82b3754ef..6fa81e072 100644
--- a/matlab/+gtsam/VisualISAMStep.m
+++ b/matlab/+gtsam/VisualISAMStep.m
@@ -27,7 +27,7 @@ for k=1:length(data.Z{nextPoseIndex})
end
%% Initial estimates for the new pose.
-prevPose = result.at(symbol('x',nextPoseIndex-1));
+prevPose = result.atPose3(symbol('x',nextPoseIndex-1));
initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry));
%% Update ISAM
diff --git a/matlab/+gtsam/load3D.m b/matlab/+gtsam/load3D.m
index 94202e0c8..d536162e1 100644
--- a/matlab/+gtsam/load3D.m
+++ b/matlab/+gtsam/load3D.m
@@ -46,9 +46,9 @@ for i=1:n
graph.add(BetweenFactorPose3(i1, i2, dpose, model));
if successive
if i2>i1
- initial.insert(i2,initial.at(i1).compose(dpose));
+ initial.insert(i2,initial.atPose3(i1).compose(dpose));
else
- initial.insert(i1,initial.at(i2).compose(dpose.inverse));
+ initial.insert(i1,initial.atPose3(i2).compose(dpose.inverse));
end
end
end
diff --git a/matlab/+gtsam/plot2DPoints.m b/matlab/+gtsam/plot2DPoints.m
index d4703a5d7..8e91fa19d 100644
--- a/matlab/+gtsam/plot2DPoints.m
+++ b/matlab/+gtsam/plot2DPoints.m
@@ -18,15 +18,18 @@ hold on
% Plot points and covariance matrices
for i = 0:keys.size-1
key = keys.at(i);
- p = values.at(key);
- if isa(p, 'gtsam.Point2')
+ try
+ p = values.atPoint2(key);
if haveMarginals
P = marginals.marginalCovariance(key);
gtsam.plotPoint2(p, linespec, P);
else
gtsam.plotPoint2(p, linespec);
end
+ catch err
+ % I guess it's not a Point2
end
+
end
if ~holdstate
diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m
index 45e7fe467..1c29213cd 100644
--- a/matlab/+gtsam/plot2DTrajectory.m
+++ b/matlab/+gtsam/plot2DTrajectory.m
@@ -8,7 +8,7 @@ function plot2DTrajectory(values, linespec, marginals)
import gtsam.*
if ~exist('linespec', 'var') || isempty(linespec)
- linespec = 'k*-';
+ linespec = 'k*-';
end
haveMarginals = exist('marginals', 'var');
@@ -25,24 +25,26 @@ plot(X,Y,linespec);
% Quiver can also be vectorized if no marginals asked
if ~haveMarginals
- C = cos(theta);
- S = sin(theta);
- quiver(X,Y,C,S,0.1,linespec);
+ C = cos(theta);
+ S = sin(theta);
+ quiver(X,Y,C,S,0.1,linespec);
else
- % plotPose2 does both quiver and covariance matrix
- keys = KeyVector(values.keys);
- for i = 0:keys.size-1
- key = keys.at(i);
- x = values.at(key);
- if isa(x, 'gtsam.Pose2')
- P = marginals.marginalCovariance(key);
- gtsam.plotPose2(x,linespec(1), P);
+ % plotPose2 does both quiver and covariance matrix
+ keys = KeyVector(values.keys);
+ for i = 0:keys.size-1
+ key = keys.at(i);
+ try
+ x = values.atPose2(key);
+ P = marginals.marginalCovariance(key);
+ gtsam.plotPose2(x,linespec(1), P);
+ catch err
+ % I guess it's not a Pose2
+ end
end
- end
end
if ~holdstate
- hold off
+ hold off
end
end
diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m
index 8feab1744..46e161807 100644
--- a/matlab/+gtsam/plot3DPoints.m
+++ b/matlab/+gtsam/plot3DPoints.m
@@ -18,14 +18,16 @@ hold on
% Plot points and covariance matrices
for i = 0:keys.size-1
key = keys.at(i);
- p = values.at(key);
- if isa(p, 'gtsam.Point3')
+ try
+ p = values.atPoint3(key);
if haveMarginals
P = marginals.marginalCovariance(key);
gtsam.plotPoint3(p, linespec, P);
else
gtsam.plotPoint3(p, linespec);
end
+ catch
+ % I guess it's not a Point3
end
end
diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m
index d24e21297..05483e589 100644
--- a/matlab/+gtsam/plot3DTrajectory.m
+++ b/matlab/+gtsam/plot3DTrajectory.m
@@ -17,39 +17,48 @@ hold on
lastIndex = [];
for i = 0:keys.size-1
key = keys.at(i);
- x = values.at(key);
- if isa(x, 'gtsam.Pose3')
+ try
+ x = values.atPose3(key);
if ~isempty(lastIndex)
% Draw line from last pose then covariance ellipse on top of
% last pose.
lastKey = keys.at(lastIndex);
- lastPose = values.at(lastKey);
- plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
+ try
+ lastPose = values.atPose3(lastKey);
+ plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
+ if haveMarginals
+ P = marginals.marginalCovariance(lastKey);
+ else
+ P = [];
+ end
+ gtsam.plotPose3(lastPose, P, scale);
+ catch err
+ warning(['no Pose3 at ' lastKey]);
+ end
+ lastIndex = i;
+ end
+ catch
+ warning(['no Pose3 at ' key]);
+ end
+
+ % Draw final pose
+ if ~isempty(lastIndex)
+ lastKey = keys.at(lastIndex);
+ try
+ lastPose = values.atPose3(lastKey);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
else
P = [];
end
gtsam.plotPose3(lastPose, P, scale);
+ catch
+ warning(['no Pose3 at ' lastIndex]);
end
- lastIndex = i;
end
-end
-
-% Draw final pose
-if ~isempty(lastIndex)
- lastKey = keys.at(lastIndex);
- lastPose = values.at(lastKey);
- if haveMarginals
- P = marginals.marginalCovariance(lastKey);
- else
- P = [];
+
+ if ~holdstate
+ hold off
end
- gtsam.plotPose3(lastPose, P, scale);
-end
-
-if ~holdstate
- hold off
-end
-
+
end
\ No newline at end of file
diff --git a/matlab/gtsam_examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m
index ef1516017..aec933d74 100644
--- a/matlab/gtsam_examples/PlanarSLAMExample.m
+++ b/matlab/gtsam_examples/PlanarSLAMExample.m
@@ -71,9 +71,9 @@ marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals);
plot2DPoints(result, 'b', marginals);
-plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-');
-plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');
-plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-');
+plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
+plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
+plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-');
axis([-0.6 4.8 -1 1])
axis equal
view(2)
diff --git a/matlab/gtsam_examples/PlanarSLAMExample_graph.m b/matlab/gtsam_examples/PlanarSLAMExample_graph.m
index 9ca88e49a..db6484c5c 100644
--- a/matlab/gtsam_examples/PlanarSLAMExample_graph.m
+++ b/matlab/gtsam_examples/PlanarSLAMExample_graph.m
@@ -22,7 +22,7 @@ model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);
[graph,initial] = load2D(datafile, model);
%% Add a Gaussian prior on a pose in the middle
-priorMean = initial.at(40);
+priorMean = initial.atPose2(40);
priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]);
graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph
diff --git a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m
index 327c64d4d..375ed645c 100644
--- a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m
+++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m
@@ -55,14 +55,14 @@ plot2DPoints(sample, [], marginals);
for j=1:2
key = symbol('l',j);
- point{j} = sample.at(key);
+ point{j} = sample.atPoint2(key);
Q{j}=marginals.marginalCovariance(key);
S{j}=chol(Q{j}); % for sampling
end
-plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-');
-plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-');
-plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-');
+plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
+plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
+plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-');
view(2); axis auto; axis equal
%% Do Sampling on point 2
diff --git a/matlab/gtsam_examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m
index 65ce28dbb..789d1c483 100644
--- a/matlab/gtsam_examples/Pose2SLAMExample.m
+++ b/matlab/gtsam_examples/Pose2SLAMExample.m
@@ -57,7 +57,7 @@ result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses
cla;
hold on
-plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-');
+plot([result.atPose2(5).x;result.atPose2(2).x],[result.atPose2(5).y;result.atPose2(2).y],'r-');
marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals);
diff --git a/matlab/gtsam_examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m
index 3d2265d76..d2676845c 100644
--- a/matlab/gtsam_examples/Pose2SLAMExample_circle.m
+++ b/matlab/gtsam_examples/Pose2SLAMExample_circle.m
@@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose2(6,1.0);
-p0 = hexagon.at(0);
-p1 = hexagon.at(1);
+p0 = hexagon.atPose2(0);
+p1 = hexagon.atPose2(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@@ -32,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance));
%% Create initial config
initial = Values;
initial.insert(0, p0);
-initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]'));
-initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]'));
-initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]'));
-initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]'));
-initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]'));
+initial.insert(1, hexagon.atPose2(1).retract([-0.1, 0.1,-0.1]'));
+initial.insert(2, hexagon.atPose2(2).retract([ 0.1,-0.1, 0.1]'));
+initial.insert(3, hexagon.atPose2(3).retract([-0.1, 0.1,-0.1]'));
+initial.insert(4, hexagon.atPose2(4).retract([ 0.1,-0.1, 0.1]'));
+initial.insert(5, hexagon.atPose2(5).retract([-0.1, 0.1,-0.1]'));
%% Plot Initial Estimate
cla
diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m
index 83ec949cc..c479278c1 100644
--- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m
+++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m
@@ -41,7 +41,7 @@ marginals = Marginals(graph, result);
toc
P={};
for i=1:result.size()-1
- pose_i = result.at(i);
+ pose_i = result.atPose2(i);
P{i}=marginals.marginalCovariance(i);
plotPose2(pose_i,'b',P{i})
end
diff --git a/matlab/gtsam_examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m
index 1d9c3b579..5a9c8bf03 100644
--- a/matlab/gtsam_examples/Pose3SLAMExample.m
+++ b/matlab/gtsam_examples/Pose3SLAMExample.m
@@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose3(6,1.0);
-p0 = hexagon.at(0);
-p1 = hexagon.at(1);
+p0 = hexagon.atPose3(0);
+p1 = hexagon.atPose3(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@@ -33,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values;
s = 0.10;
initial.insert(0, p0);
-initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
-initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
-initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
-initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
-initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
+initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
+initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
+initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
+initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
+initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
%% Plot Initial Estimate
cla
diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m
index 39e48c204..de6f9e86d 100644
--- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m
+++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m
@@ -28,7 +28,7 @@ model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.
%% Plot Initial Estimate
cla
-first = initial.at(0);
+first = initial.atPose3(0);
plot3(first.x(),first.y(),first.z(),'r*'); hold on
plot3DTrajectory(initial,'g-',false);
drawnow;
diff --git a/matlab/gtsam_examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m
index b77733abd..464448335 100644
--- a/matlab/gtsam_examples/StereoVOExample_large.m
+++ b/matlab/gtsam_examples/StereoVOExample_large.m
@@ -45,7 +45,7 @@ for i=1:size(measurements,1)
if ~initial.exists(symbol('l',sf(2)))
% 3D landmarks are stored in camera coordinates: transform
% to world coordinates using the respective initial pose
- pose = initial.at(symbol('x', sf(1)));
+ pose = initial.atPose3(symbol('x', sf(1)));
world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8)));
initial.insert(symbol('l',sf(2)), world_point);
end
@@ -54,7 +54,7 @@ toc
%% add a constraint on the starting pose
key = symbol('x',1);
-first_pose = initial.at(key);
+first_pose = initial.atPose3(key);
graph.add(NonlinearEqualityPose3(key, first_pose));
%% optimize
diff --git a/matlab/gtsam_tests/testLocalizationExample.m b/matlab/gtsam_tests/testLocalizationExample.m
index 5f1d89c99..ed091ea71 100644
--- a/matlab/gtsam_tests/testLocalizationExample.m
+++ b/matlab/gtsam_tests/testLocalizationExample.m
@@ -29,7 +29,7 @@ groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
for i=1:3
- graph.add(PriorFactorPose2(i, groundTruth.at(i), model));
+ graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
end
%% Initialize to noisy points
@@ -46,7 +46,7 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
P={};
for i=1:result.size()
- pose_i = result.at(i);
- CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4));
+ pose_i = result.atPose2(i);
+ CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4));
P{i}=marginals.marginalCovariance(i);
end
diff --git a/matlab/gtsam_tests/testOdometryExample.m b/matlab/gtsam_tests/testOdometryExample.m
index 442b36801..9bd4762a7 100644
--- a/matlab/gtsam_tests/testOdometryExample.m
+++ b/matlab/gtsam_tests/testOdometryExample.m
@@ -39,5 +39,5 @@ marginals = Marginals(graph, result);
marginals.marginalCovariance(1);
%% Check first pose equality
-pose_1 = result.at(1);
+pose_1 = result.atPose2(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m
index 7694a1a0b..d3cab5d1f 100644
--- a/matlab/gtsam_tests/testPlanarSLAMExample.m
+++ b/matlab/gtsam_tests/testPlanarSLAMExample.m
@@ -60,10 +60,10 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
%% Check first pose and point equality
-pose_1 = result.at(symbol('x',1));
+pose_1 = result.atPose2(symbol('x',1));
marginals.marginalCovariance(symbol('x',1));
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
-point_1 = result.at(symbol('l',1));
+point_1 = result.atPoint2(symbol('l',1));
marginals.marginalCovariance(symbol('l',1));
CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));
diff --git a/matlab/gtsam_tests/testPose2SLAMExample.m b/matlab/gtsam_tests/testPose2SLAMExample.m
index 8c70c27e7..72e5331f2 100644
--- a/matlab/gtsam_tests/testPose2SLAMExample.m
+++ b/matlab/gtsam_tests/testPose2SLAMExample.m
@@ -57,6 +57,6 @@ result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
P = marginals.marginalCovariance(1);
-pose_1 = result.at(1);
+pose_1 = result.atPose2(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
diff --git a/matlab/gtsam_tests/testPose3SLAMExample.m b/matlab/gtsam_tests/testPose3SLAMExample.m
index dafad4e47..51ba69240 100644
--- a/matlab/gtsam_tests/testPose3SLAMExample.m
+++ b/matlab/gtsam_tests/testPose3SLAMExample.m
@@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose3(6,1.0);
-p0 = hexagon.at(0);
-p1 = hexagon.at(1);
+p0 = hexagon.atPose3(0);
+p1 = hexagon.atPose3(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@@ -33,17 +33,17 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values;
s = 0.10;
initial.insert(0, p0);
-initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
-initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
-initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
-initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
-initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
+initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
+initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
+initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
+initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
+initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
%% optimize
optimizer = LevenbergMarquardtOptimizer(fg, initial);
result = optimizer.optimizeSafely;
-pose_1 = result.at(1);
+pose_1 = result.atPose3(1);
CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4));
diff --git a/matlab/gtsam_tests/testSFMExample.m b/matlab/gtsam_tests/testSFMExample.m
index 2b04be8f1..985cbdb2c 100644
--- a/matlab/gtsam_tests/testSFMExample.m
+++ b/matlab/gtsam_tests/testSFMExample.m
@@ -63,11 +63,11 @@ marginals.marginalCovariance(symbol('x',1));
%% Check optimized results, should be equal to ground truth
for i=1:size(truth.cameras,2)
- pose_i = result.at(symbol('x',i));
+ pose_i = result.atPose3(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
- point_j = result.at(symbol('p',j));
+ point_j = result.atPoint3(symbol('p',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end
diff --git a/matlab/gtsam_tests/testStereoVOExample.m b/matlab/gtsam_tests/testStereoVOExample.m
index e2d6f2479..218d0ace1 100644
--- a/matlab/gtsam_tests/testStereoVOExample.m
+++ b/matlab/gtsam_tests/testStereoVOExample.m
@@ -61,8 +61,8 @@ optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize();
%% check equality for the first pose and point
-pose_x1 = result.at(x1);
+pose_x1 = result.atPose3(x1);
CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
-point_l1 = result.at(l1);
+point_l1 = result.atPoint3(l1);
CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));
\ No newline at end of file
diff --git a/matlab/gtsam_tests/testVisualISAMExample.m b/matlab/gtsam_tests/testVisualISAMExample.m
index 40aca458e..223e823a6 100644
--- a/matlab/gtsam_tests/testVisualISAMExample.m
+++ b/matlab/gtsam_tests/testVisualISAMExample.m
@@ -45,11 +45,11 @@ for frame_i=3:options.nrCameras
end
for i=1:size(truth.cameras,2)
- pose_i = result.at(symbol('x',i));
+ pose_i = result.atPose3(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
- point_j = result.at(symbol('l',j));
+ point_j = result.atPoint3(symbol('l',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end
diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m
index c379179c5..e3705948d 100644
--- a/matlab/gtsam_tests/test_gtsam.m
+++ b/matlab/gtsam_tests/test_gtsam.m
@@ -30,11 +30,11 @@ testStereoVOExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
-display 'Starting: testSerialization'
-testSerialization
-
display 'Starting: testUtilities'
testUtilities
+display 'Starting: testSerialization'
+testSerialization
+
% end of tests
display 'Tests complete!'
diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp
index d76556e4a..4989afb0d 100644
--- a/wrap/Argument.cpp
+++ b/wrap/Argument.cpp
@@ -28,34 +28,53 @@
using namespace std;
using namespace wrap;
+/* ************************************************************************* */
+Argument Argument::expandTemplate(const TemplateSubstitution& ts) const {
+ Argument instArg = *this;
+ instArg.type = ts(type);
+ return instArg;
+}
+
+/* ************************************************************************* */
+ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const {
+ ArgumentList instArgList;
+ BOOST_FOREACH(const Argument& arg, *this) {
+ Argument instArg = arg.expandTemplate(ts);
+ instArgList.push_back(instArg);
+ }
+ return instArgList;
+}
+
/* ************************************************************************* */
string Argument::matlabClass(const string& delim) const {
string result;
- BOOST_FOREACH(const string& ns, namespaces)
+ BOOST_FOREACH(const string& ns, type.namespaces)
result += ns + delim;
- if (type == "string" || type == "unsigned char" || type == "char")
+ if (type.name == "string" || type.name == "unsigned char"
+ || type.name == "char")
return result + "char";
- if (type == "Vector" || type == "Matrix")
+ if (type.name == "Vector" || type.name == "Matrix")
return result + "double";
- if (type == "int" || type == "size_t")
+ if (type.name == "int" || type.name == "size_t")
return result + "numeric";
- if (type == "bool")
+ if (type.name == "bool")
return result + "logical";
- return result + type;
+ return result + type.name;
}
/* ************************************************************************* */
bool Argument::isScalar() const {
- return (type == "bool" || type == "char" || type == "unsigned char"
- || type == "int" || type == "size_t" || type == "double");
+ return (type.name == "bool" || type.name == "char"
+ || type.name == "unsigned char" || type.name == "int"
+ || type.name == "size_t" || type.name == "double");
}
/* ************************************************************************* */
void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << " ";
- string cppType = qualifiedType("::");
- string matlabUniqueType = qualifiedType();
+ string cppType = type.qualifiedName("::");
+ string matlabUniqueType = type.qualifiedName();
if (is_ptr)
// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
@@ -78,14 +97,6 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << ");" << endl;
}
-/* ************************************************************************* */
-string Argument::qualifiedType(const string& delim) const {
- string result;
- BOOST_FOREACH(const string& ns, namespaces)
- result += ns + delim;
- return result + type;
-}
-
/* ************************************************************************* */
string ArgumentList::types() const {
string str;
@@ -93,7 +104,7 @@ string ArgumentList::types() const {
BOOST_FOREACH(Argument arg, *this) {
if (!first)
str += ",";
- str += arg.type;
+ str += arg.type.name;
first = false;
}
return str;
@@ -105,14 +116,14 @@ string ArgumentList::signature() const {
bool cap = false;
BOOST_FOREACH(Argument arg, *this) {
- BOOST_FOREACH(char ch, arg.type)
+ BOOST_FOREACH(char ch, arg.type.name)
if (isupper(ch)) {
sig += ch;
//If there is a capital letter, we don't want to read it below
cap = true;
}
if (!cap)
- sig += arg.type[0];
+ sig += arg.type.name[0];
//Reset to default
cap = false;
}
@@ -136,7 +147,8 @@ string ArgumentList::names() const {
/* ************************************************************************* */
bool ArgumentList::allScalar() const {
BOOST_FOREACH(Argument arg, *this)
- if (!arg.isScalar()) return false;
+ if (!arg.isScalar())
+ return false;
return true;
}
@@ -158,42 +170,43 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const {
BOOST_FOREACH(Argument arg, *this) {
if (!first)
file.oss << ", ";
- file.oss << arg.type << " " << arg.name;
+ file.oss << arg.type.name << " " << arg.name;
first = false;
}
file.oss << ")";
}
/* ************************************************************************* */
-void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal,
- const string& wrapperName, int id, bool staticMethod) const {
- returnVal.emit_matlab(file);
- file.oss << wrapperName << "(" << id;
+void ArgumentList::emit_call(FileWriter& proxyFile,
+ const ReturnValue& returnVal, const string& wrapperName, int id,
+ bool staticMethod) const {
+ returnVal.emit_matlab(proxyFile);
+ proxyFile.oss << wrapperName << "(" << id;
if (!staticMethod)
- file.oss << ", this";
- file.oss << ", varargin{:});\n";
+ proxyFile.oss << ", this";
+ proxyFile.oss << ", varargin{:});\n";
}
/* ************************************************************************* */
-void ArgumentList::emit_conditional_call(FileWriter& file,
+void ArgumentList::emit_conditional_call(FileWriter& proxyFile,
const ReturnValue& returnVal, const string& wrapperName, int id,
bool staticMethod) const {
// Check nr of arguments
- file.oss << "if length(varargin) == " << size();
+ proxyFile.oss << "if length(varargin) == " << size();
if (size() > 0)
- file.oss << " && ";
- // ...and their types
+ proxyFile.oss << " && ";
+ // ...and their type.names
bool first = true;
for (size_t i = 0; i < size(); i++) {
if (!first)
- file.oss << " && ";
- file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".")
- << "')";
+ proxyFile.oss << " && ";
+ proxyFile.oss << "isa(varargin{" << i + 1 << "},'"
+ << (*this)[i].matlabClass(".") << "')";
first = false;
}
- file.oss << "\n";
+ proxyFile.oss << "\n";
// output call to C++ wrapper
- file.oss << " ";
- emit_call(file, returnVal, wrapperName, id, staticMethod);
+ proxyFile.oss << " ";
+ emit_call(proxyFile, returnVal, wrapperName, id, staticMethod);
}
/* ************************************************************************* */
diff --git a/wrap/Argument.h b/wrap/Argument.h
index 6f791978a..02f104418 100644
--- a/wrap/Argument.h
+++ b/wrap/Argument.h
@@ -19,36 +19,39 @@
#pragma once
+#include "TemplateSubstitution.h"
#include "FileWriter.h"
#include "ReturnValue.h"
-#include
-#include
-
namespace wrap {
/// Argument class
struct Argument {
+ Qualified type;
bool is_const, is_ref, is_ptr;
- std::string type;
std::string name;
- std::vector namespaces;
Argument() :
is_const(false), is_ref(false), is_ptr(false) {
}
+ Argument expandTemplate(const TemplateSubstitution& ts) const;
+
/// return MATLAB class for use in isa(x,class)
std::string matlabClass(const std::string& delim = "") const;
/// Check if will be unwrapped using scalar login in wrap/matlab.h
bool isScalar() const;
- /// adds namespaces to type
- std::string qualifiedType(const std::string& delim = "") const;
-
/// MATLAB code generation, MATLAB to C++
void matlab_unwrap(FileWriter& file, const std::string& matlabName) const;
+
+ friend std::ostream& operator<<(std::ostream& os, const Argument& arg) {
+ os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "")
+ << (arg.is_ref ? "&" : "");
+ return os;
+ }
+
};
/// Argument list is just a container with Arguments
@@ -66,6 +69,8 @@ struct ArgumentList: public std::vector {
/// Check if all arguments scalar
bool allScalar() const;
+ ArgumentList expandTemplate(const TemplateSubstitution& ts) const;
+
// MATLAB code generation:
/**
@@ -83,25 +88,47 @@ struct ArgumentList: public std::vector {
void emit_prototype(FileWriter& file, const std::string& name) const;
/**
- * emit emit MATLAB call to wrapper
- * @param file output stream
+ * emit emit MATLAB call to proxy
+ * @param proxyFile output stream
* @param returnVal the return value
* @param wrapperName of method or function
* @param staticMethod flag to emit "this" in call
*/
- void emit_call(FileWriter& file, const ReturnValue& returnVal,
+ void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal,
const std::string& wrapperName, int id, bool staticMethod = false) const;
/**
- * emit conditional MATLAB call to wrapper (checking arguments first)
- * @param file output stream
+ * emit conditional MATLAB call to proxy (checking arguments first)
+ * @param proxyFile output stream
* @param returnVal the return value
* @param wrapperName of method or function
* @param staticMethod flag to emit "this" in call
*/
- void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal,
- const std::string& wrapperName, int id, bool staticMethod = false) const;
+ void emit_conditional_call(FileWriter& proxyFile,
+ const ReturnValue& returnVal, const std::string& wrapperName, int id,
+ bool staticMethod = false) const;
+
+ friend std::ostream& operator<<(std::ostream& os,
+ const ArgumentList& argList) {
+ os << "(";
+ if (argList.size() > 0)
+ os << argList.front();
+ if (argList.size() > 1)
+ for (size_t i = 1; i < argList.size(); i++)
+ os << ", " << argList[i];
+ os << ")";
+ return os;
+ }
+
};
+template
+inline void verifyArguments(const std::vector& validArgs,
+ const std::map& vt) {
+ typedef typename std::map::value_type NamedMethod;
+ BOOST_FOREACH(const NamedMethod& namedMethod, vt)
+ namedMethod.second.verifyArguments(validArgs);
+}
+
} // \namespace wrap
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index 075c98811..3a3432eb3 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -33,7 +33,7 @@ using namespace std;
using namespace wrap;
/* ************************************************************************* */
-void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
+void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
vector& functionNames) const {
@@ -41,17 +41,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
createNamespaceStructure(namespaces, toolboxPath);
// open destination classFile
- string classFile = toolboxPath;
- if (!namespaces.empty())
- classFile += "/+" + wrap::qualifiedName("/+", namespaces);
- classFile += "/" + name + ".m";
+ string classFile = matlabName(toolboxPath);
FileWriter proxyFile(classFile, verbose_, "%");
// get the name of actual matlab object
- const string matlabQualName = qualifiedName("."), matlabUniqueName =
- qualifiedName(), cppName = qualifiedName("::");
- const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent);
- const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
+ const string matlabQualName = qualifiedName(".");
+ const string matlabUniqueName = qualifiedName();
+ const string cppName = qualifiedName("::");
+ const string matlabBaseName = qualifiedParent.qualifiedName(".");
+ const string cppBaseName = qualifiedParent.qualifiedName("::");
// emit class proxy code
// we want our class to inherit the handle class for memory purposes
@@ -75,13 +73,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName,
functionNames);
wrapperFile.oss << "\n";
+
// Regular constructors
- BOOST_FOREACH(ArgumentList a, constructor.args_list) {
+ for (size_t i = 0; i < constructor.nrOverloads(); i++) {
+ ArgumentList args = constructor.argumentList(i);
const int id = (int) functionNames.size();
constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(),
- id, a);
+ id, args);
const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
- cppName, matlabUniqueName, cppBaseName, id, a);
+ cppName, matlabUniqueName, cppBaseName, id, args);
wrapperFile.oss << "\n";
functionNames.push_back(wrapFunctionName);
}
@@ -144,19 +144,14 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
proxyFile.emit(true);
}
-/* ************************************************************************* */
-string Class::qualifiedName(const string& delim) const {
- return ::wrap::qualifiedName(delim, namespaces, name);
-}
-
/* ************************************************************************* */
void Class::pointer_constructor_fragments(FileWriter& proxyFile,
- FileWriter& wrapperFile, const string& wrapperName,
+ FileWriter& wrapperFile, Str wrapperName,
vector& functionNames) const {
- const string matlabUniqueName = qualifiedName(), cppName = qualifiedName(
- "::");
- const string baseCppName = wrap::qualifiedName("::", qualifiedParent);
+ const string matlabUniqueName = qualifiedName();
+ const string cppName = qualifiedName("::");
+ const string baseCppName = qualifiedParent.qualifiedName("::");
const int collectorInsertId = (int) functionNames.size();
const string collectorInsertFunctionName = matlabUniqueName
@@ -247,128 +242,126 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
}
/* ************************************************************************* */
-vector expandArgumentListsTemplate(
- const vector& argLists, const string& templateArg,
- const vector& instName,
- const std::vector& expandedClassNamespace,
- const string& expandedClassName) {
- vector result;
- BOOST_FOREACH(const ArgumentList& argList, argLists) {
- ArgumentList instArgList;
- BOOST_FOREACH(const Argument& arg, argList) {
- Argument instArg = arg;
- if (arg.type == templateArg) {
- instArg.namespaces.assign(instName.begin(), instName.end() - 1);
- instArg.type = instName.back();
- } 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
-map expandMethodTemplate(const map& methods,
- const string& templateArg, const vector& instName,
- const std::vector& expandedClassNamespace,
- const string& expandedClassName) {
- map result;
- typedef pair Name_Method;
- BOOST_FOREACH(const Name_Method& name_method, methods) {
- const METHOD& method = name_method.second;
- METHOD instMethod = method;
- instMethod.argLists = expandArgumentListsTemplate(method.argLists,
- templateArg, instName, 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& instName,
- const std::vector& expandedClassNamespace,
- const string& expandedClassName) {
- Class inst;
- inst.name = cls.name;
- inst.templateArgs = cls.templateArgs;
- inst.typedefName = cls.typedefName;
- inst.isVirtual = cls.isVirtual;
- inst.isSerializable = cls.isSerializable;
- inst.qualifiedParent = cls.qualifiedParent;
- inst.methods = expandMethodTemplate(cls.methods, templateArg, instName,
- expandedClassNamespace, expandedClassName);
- inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg,
- instName, expandedClassNamespace, expandedClassName);
- inst.namespaces = cls.namespaces;
- inst.constructor = cls.constructor;
- inst.constructor.args_list = expandArgumentListsTemplate(
- cls.constructor.args_list, templateArg, instName, expandedClassNamespace,
- expandedClassName);
- inst.constructor.name = inst.name;
- inst.deconstructor = cls.deconstructor;
+Class Class::expandTemplate(const TemplateSubstitution& ts) const {
+ Class inst = *this;
+ inst.methods = expandMethodTemplate(methods, ts);
+ inst.static_methods = expandMethodTemplate(static_methods, ts);
+ inst.constructor = constructor.expandTemplate(ts);
inst.deconstructor.name = inst.name;
- inst.verbose_ = cls.verbose_;
return inst;
}
/* ************************************************************************* */
-vector Class::expandTemplate(const string& templateArg,
- const vector >& instantiations) const {
+vector Class::expandTemplate(Str templateArg,
+ const vector& instantiations) const {
vector result;
- BOOST_FOREACH(const vector& instName, instantiations) {
- const string expandedName = name + instName.back();
- Class inst = expandClassTemplate(*this, templateArg, instName,
- this->namespaces, expandedName);
- inst.name = expandedName;
+ BOOST_FOREACH(const Qualified& instName, instantiations) {
+ Qualified expandedClass = (Qualified) (*this);
+ expandedClass.name += instName.name;
+ const TemplateSubstitution ts(templateArg, instName, expandedClass);
+ Class inst = expandTemplate(ts);
+ inst.name = expandedClass.name;
inst.templateArgs.clear();
- inst.typedefName = qualifiedName("::") + "<"
- + wrap::qualifiedName("::", instName) + ">";
+ inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::")
+ + ">";
result.push_back(inst);
}
return result;
}
/* ************************************************************************* */
-Class Class::expandTemplate(const string& templateArg,
- const vector& instantiation,
- const std::vector& expandedClassNamespace,
- const string& expandedClassName) const {
- return expandClassTemplate(*this, templateArg, instantiation,
- expandedClassNamespace, expandedClassName);
+void Class::addMethod(bool verbose, bool is_const, Str methodName,
+ const ArgumentList& argumentList, const ReturnValue& returnValue,
+ Str templateArgName, const vector& templateArgValues) {
+ // Check if templated
+ if (!templateArgName.empty() && templateArgValues.size() > 0) {
+ // Create method to expand
+ // For all values of the template argument, create a new method
+ BOOST_FOREACH(const Qualified& instName, templateArgValues) {
+ const TemplateSubstitution ts(templateArgName, instName, this->name);
+ // substitute template in arguments
+ ArgumentList expandedArgs = argumentList.expandTemplate(ts);
+ // do the same for return type
+ ReturnValue expandedRetVal = returnValue.expandTemplate(ts);
+ // Now stick in new overload stack with expandedMethodName key
+ // but note we use the same, unexpanded methodName in overload
+ string expandedMethodName = methodName + instName.name;
+ methods[expandedMethodName].addOverload(verbose, is_const, methodName,
+ expandedArgs, expandedRetVal, instName);
+ }
+ } else
+ // just add overload
+ methods[methodName].addOverload(verbose, is_const, methodName, argumentList,
+ returnValue);
}
/* ************************************************************************* */
-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& validTypes, bool& hasSerialiable) const {
+
+ hasSerialiable |= isSerializable;
+
+ // verify all of the function arguments
+ //TODO:verifyArguments(validTypes, constructor.args_list);
+ verifyArguments(validTypes, static_methods);
+ verifyArguments(validTypes, methods);
+
+ // verify function return types
+ verifyReturnTypes(validTypes, static_methods);
+ verifyReturnTypes(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& classes) {
+
+ if (!cls.qualifiedParent.empty()) {
+
+ // Find parent
+ BOOST_FOREACH(const Class& parent, classes) {
+ // We found a parent class for our parent, TODO improve !
+ if (parent.name == cls.qualifiedParent.name) {
+ methods.insert(parent.methods.begin(), parent.methods.end());
+ appendInheritedMethods(parent, classes);
+ }
+ }
+ }
+}
+
+/* ************************************************************************* */
+string Class::getTypedef() const {
string result;
- BOOST_FOREACH(const string& namesp, namespaces) {
+ BOOST_FOREACH(Str namesp, namespaces) {
result += ("namespace " + namesp + " { ");
}
result += ("typedef " + typedefName + " " + name + ";");
@@ -379,43 +372,22 @@ std::string Class::getTypedef() const {
}
/* ************************************************************************* */
-
void Class::comment_fragment(FileWriter& proxyFile) const {
proxyFile.oss << "%class " << name << ", see Doxygen page for details\n";
proxyFile.oss
<< "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
- if (!constructor.args_list.empty())
- proxyFile.oss << "%\n%-------Constructors-------\n";
- BOOST_FOREACH(ArgumentList argList, constructor.args_list) {
- proxyFile.oss << "%";
- argList.emit_prototype(proxyFile, name);
- proxyFile.oss << "\n";
- }
+ constructor.comment_fragment(proxyFile);
if (!methods.empty())
proxyFile.oss << "%\n%-------Methods-------\n";
- BOOST_FOREACH(const Methods::value_type& name_m, methods) {
- const Method& m = name_m.second;
- BOOST_FOREACH(ArgumentList argList, m.argLists) {
- proxyFile.oss << "%";
- argList.emit_prototype(proxyFile, m.name);
- proxyFile.oss << " : returns "
- << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl;
- }
- }
+ BOOST_FOREACH(const Methods::value_type& name_m, methods)
+ name_m.second.comment_fragment(proxyFile);
if (!static_methods.empty())
proxyFile.oss << "%\n%-------Static Methods-------\n";
- BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
- const StaticMethod& m = name_m.second;
- BOOST_FOREACH(ArgumentList argList, m.argLists) {
- proxyFile.oss << "%";
- argList.emit_prototype(proxyFile, m.name);
- proxyFile.oss << " : returns "
- << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl;
- }
- }
+ BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods)
+ name_m.second.comment_fragment(proxyFile);
if (hasSerialization) {
proxyFile.oss << "%\n%-------Serialization Interface-------\n";
@@ -429,23 +401,24 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
/* ************************************************************************* */
void Class::serialization_fragments(FileWriter& proxyFile,
- FileWriter& wrapperFile, const std::string& wrapperName,
- std::vector& functionNames) const {
+ FileWriter& wrapperFile, Str wrapperName,
+ vector& functionNames) const {
//void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
//{
// typedef boost::shared_ptr Shared;
// checkArguments("string_serialize",nargout,nargin-1,0);
// Shared obj = unwrap_shared_ptr(in[0], "ptr_Point3");
-// std::ostringstream out_archive_stream;
+// ostringstream out_archive_stream;
// boost::archive::text_oarchive out_archive(out_archive_stream);
// out_archive << *obj;
// out[0] = wrap< string >(out_archive_stream.str());
//}
int serialize_id = functionNames.size();
- const string matlabQualName = qualifiedName("."), matlabUniqueName =
- qualifiedName(), cppClassName = qualifiedName("::");
+ const string matlabQualName = qualifiedName(".");
+ const string matlabUniqueName = qualifiedName();
+ const string cppClassName = qualifiedName("::");
const string wrapFunctionNameSerialize = matlabUniqueName
+ "_string_serialize_" + boost::lexical_cast(serialize_id);
functionNames.push_back(wrapFunctionNameSerialize);
@@ -469,7 +442,7 @@ void Class::serialization_fragments(FileWriter& proxyFile,
<< ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
// Serialization boilerplate
- wrapperFile.oss << " std::ostringstream out_archive_stream;\n";
+ wrapperFile.oss << " ostringstream out_archive_stream;\n";
wrapperFile.oss
<< " boost::archive::text_oarchive out_archive(out_archive_stream);\n";
wrapperFile.oss << " out_archive << *obj;\n";
@@ -520,22 +493,23 @@ void Class::serialization_fragments(FileWriter& proxyFile,
/* ************************************************************************* */
void Class::deserialization_fragments(FileWriter& proxyFile,
- FileWriter& wrapperFile, const std::string& wrapperName,
- std::vector& functionNames) const {
+ FileWriter& wrapperFile, Str wrapperName,
+ vector& functionNames) const {
//void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
//{
// typedef boost::shared_ptr Shared;
// checkArguments("Point3.string_deserialize",nargout,nargin,1);
// string serialized = unwrap< string >(in[0]);
- // std::istringstream in_archive_stream(serialized);
+ // istringstream in_archive_stream(serialized);
// boost::archive::text_iarchive in_archive(in_archive_stream);
// Shared output(new Point3());
// in_archive >> *output;
// out[0] = wrap_shared_ptr(output,"Point3", false);
//}
int deserialize_id = functionNames.size();
- const string matlabQualName = qualifiedName("."), matlabUniqueName =
- qualifiedName(), cppClassName = qualifiedName("::");
+ const string matlabQualName = qualifiedName(".");
+ const string matlabUniqueName = qualifiedName();
+ const string cppClassName = qualifiedName("::");
const string wrapFunctionNameDeserialize = matlabUniqueName
+ "_string_deserialize_" + boost::lexical_cast(deserialize_id);
functionNames.push_back(wrapFunctionNameDeserialize);
@@ -553,7 +527,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
// string argument with deserialization boilerplate
wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n";
- wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n";
+ wrapperFile.oss << " istringstream in_archive_stream(serialized);\n";
wrapperFile.oss
<< " boost::archive::text_iarchive in_archive(in_archive_stream);\n";
wrapperFile.oss << " Shared output(new " << cppClassName << "());\n";
@@ -604,7 +578,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
}
/* ************************************************************************* */
-std::string Class::getSerializationExport() const {
+string Class::getSerializationExport() const {
//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal");
return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \""
+ qualifiedName() + "\");";
diff --git a/wrap/Class.h b/wrap/Class.h
index 2ca976f66..6b9119316 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -19,67 +19,116 @@
#pragma once
-#include
-#include