diff --git a/.cproject b/.cproject
index b1ca9e152..e5b781d91 100644
--- a/.cproject
+++ b/.cproject
@@ -769,6 +769,14 @@
true
true
+
+ make
+ -j2
+ tests/testSimulated2D.run
+ true
+ true
+ true
+
make
-j2
@@ -1720,10 +1728,10 @@
true
true
-
+
make
-j2
- PlanarSLAMExample.run
+ PlanarSLAMExample_easy.run
true
true
true
diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp
index f840c443d..44200af39 100644
--- a/examples/PlanarSLAMExample_easy.cpp
+++ b/examples/PlanarSLAMExample_easy.cpp
@@ -86,8 +86,10 @@ int main(int argc, char** argv) {
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
- Values result = optimize(graph, initialEstimate);
- result.print("final result");
+// Values result = optimize(graph, initialEstimate);
+// result.print("final result");
+ boost::shared_ptr result = graph.optimize(initialEstimate);
+ result->print("final result");
return 0;
}
diff --git a/examples/matlab/PlanarSLAMExample_easy.m b/examples/matlab/PlanarSLAMExample_easy.m
new file mode 100644
index 000000000..8d9c250b0
--- /dev/null
+++ b/examples/matlab/PlanarSLAMExample_easy.m
@@ -0,0 +1,75 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+%
+% @brief Simple robotics example using the pre-built planar SLAM domain
+% @author Alex Cunningham
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Assumptions
+% - All values are axis aligned
+% - Robot poses are facing along the X axis (horizontal, to the right in images)
+% - We have bearing and range information for measurements
+% - We have full odometry for measurements
+% - The robot and landmarks are on a grid, moving 2 meters each step
+% - Landmarks are 2 meters away from the robot trajectory
+
+%% Create keys for variables
+x1 = 1; x2 = 2; x3 = 3;
+l1 = 1; l2 = 2;
+
+%% Create graph container and add factors to it
+graph = PlanarSLAMGraph;
+
+%% Add prior
+% gaussian for prior
+prior_model = SharedDiagonal([0.3; 0.3; 0.1]);
+prior_measurement = Pose2(0.0, 0.0, 0.0); % prior at origin
+graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
+
+%% Add odometry
+% general noisemodel for odometry
+odom_model = SharedDiagonal([0.2; 0.2; 0.1]);
+odom_measurement = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+graph.addOdometry(x1, x2, odom_measurement, odom_model);
+graph.addOdometry(x2, x3, odom_measurement, odom_model);
+
+%% Add measurements
+% general noisemodel for measurements
+meas_model = SharedDiagonal([0.1; 0.2]);
+
+% create the measurement values - indices are (pose id, landmark id)
+degrees = pi/180;
+bearing11 = Rot2(45*degrees);
+bearing21 = Rot2(90*degrees);
+bearing32 = Rot2(90*degrees);
+range11 = sqrt(4+4);
+range21 = 2.0;
+range32 = 2.0;
+
+% create bearing/range factors and add them
+graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
+graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
+graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
+
+% print
+graph.print('full graph');
+
+%% Initialize to noisy points
+initialEstimate = PlanarSLAMValues;
+initialEstimate.insertPose(x1, Pose2(0.5, 0.0, 0.2));
+initialEstimate.insertPose(x2, Pose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(x3, Pose2(4.1, 0.1, 0.1));
+initialEstimate.insertPoint(l1, Landmark2(1.8, 2.1));
+initialEstimate.insertPoint(l2, Landmark2(4.1, 1.8));
+
+initialEstimate.print('initial estimate');
+
+%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+result = graph.optimize(initialEstimate);
+result.print('final result');
diff --git a/gtsam.h b/gtsam.h
index f805460a7..cf59cc3b2 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -19,6 +19,11 @@ class Point3 {
class Rot2 {
Rot2();
+ Rot2(double theta);
+ void print(string s) const;
+ bool equals(const Rot2& pose, double tol) const;
+ double c() const;
+ double s() const;
};
class Pose2 {
@@ -106,3 +111,40 @@ class GaussianFactorGraph {
void combine(const GaussianFactorGraph& lfg);
};
+class Landmark2 {
+ Landmark2();
+ Landmark2(double x, double y);
+ void print(string s) const;
+ double x();
+ double y();
+};
+
+class Ordering{
+ void print(string s) const;
+ bool equals(const Ordering& ord, double tol) const;
+};
+
+class PlanarSLAMValues {
+ PlanarSLAMValues();
+ void print(string s) const;
+ void insertPose(int key, const Pose2& pose);
+ void insertPoint(int key, const Point2& point);
+};
+
+class PlanarSLAMGraph {
+ PlanarSLAMGraph();
+ double error(const PlanarSLAMValues& c) const;
+ Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const;
+ void print(string s) const;
+ void addPrior(size_t i, const Pose2& p, const SharedNoiseModel& model);
+ void addPoseConstraint(size_t i, const Pose2& p);
+ void addOdometry(size_t i, size_t j, const Pose2& z,
+ const SharedNoiseModel& model);
+ void addBearing(size_t i, size_t j, const Rot2& z,
+ const SharedNoiseModel& model);
+ void addRange(size_t i, size_t j, double z, const SharedNoiseModel& model);
+ void addBearingRange(size_t i, size_t j, const Rot2& z1, double z2,
+ const SharedNoiseModel& model);
+ PlanarSLAMValues* optimize(const PlanarSLAMValues& initialEstimate);
+};
+
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index a41968f79..605163d5d 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -14,10 +14,12 @@
* @brief 2D Pose
*/
-#include
+#include
#include
#include
-#include
+#include
+#include
+#include
using namespace std;
@@ -55,7 +57,7 @@ namespace gtsam {
assert(xi.size() == 3);
Point2 v(xi(0),xi(1));
double w = xi(2);
- if (fabs(w) < 1e-10)
+ if (std::abs(w) < 1e-10)
return Pose2(xi[0], xi[1], xi[2]);
else {
Rot2 R(Rot2::fromAngle(w));
@@ -70,7 +72,7 @@ namespace gtsam {
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
- if (fabs(w) < 1e-10)
+ if (std::abs(w) < 1e-10)
return Vector_(3, t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
@@ -173,13 +175,13 @@ namespace gtsam {
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
+ // Assert that R1 and R2 are normalized
+ assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
+
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
- // Assert that R1 and R2 are normalized
- assert(fabs(c1*c1 + s1*s1 - 1.0) < 1e-5 && fabs(c2*c2 + s2*s2 - 1.0) < 1e-5);
-
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
@@ -228,16 +230,16 @@ namespace gtsam {
boost::optional H1, boost::optional H2) const {
if (!H1 && !H2) return transform_to(point).norm();
Point2 d = transform_to(point, H1, H2);
- double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
+ double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2);
Matrix D_result_d;
- if(fabs(n) > 1e-10)
- D_result_d = Matrix_(1, 2, x / n, y / n);
+ if(std::abs(r) > 1e-10)
+ D_result_d = Matrix_(1, 2, x / r, y / r);
else {
D_result_d = Matrix_(1,2, 1.0, 1.0);
}
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
- return n;
+ return r;
}
/* ************************************************************************* */
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 7304ddcaf..b2ceafca0 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -23,170 +23,200 @@
namespace gtsam {
- /**
- * Rotation matrix
- * NOTE: the angle theta is in radians unless explicitly stated
- * @ingroup geometry
- */
- class Rot2: public Lie {
-
- public:
- /** get the dimension by the type */
- static const size_t dimension = 1;
-
- private:
-
- /** we store cos(theta) and sin(theta) */
- double c_, s_;
-
- /** private constructor from cos/sin */
- inline Rot2(double c, double s) : c_(c), s_(s) {}
-
- /** normalize to make sure cos and sin form unit vector */
- Rot2& normalize();
-
- public:
-
- /** default constructor, zero rotation */
- Rot2() : c_(1.0), s_(0.0) {}
-
- /** "named constructors" */
-
- /** Named constructor from angle == exponential map at identity - theta is in radians*/
- static Rot2 fromAngle(double theta) {
- return Rot2(cos(theta), sin(theta));
- }
-
- /** Named constructor from angle in degrees */
- static Rot2 fromDegrees(double theta) {
- const double degree = M_PI / 180;
- return fromAngle(theta * degree);
- }
-
- /** Named constructor from cos(theta),sin(theta) pair, will *not* normalize! */
- static Rot2 fromCosSin(double c, double s);
-
/**
- * Named constructor with derivative
- * Calculate relative bearing to a landmark in local coordinate frame
- * @param point 2D location of landmark
- * @param H optional reference for Jacobian
- * @return 2D rotation \in SO(2)
+ * Rotation matrix
+ * NOTE: the angle theta is in radians unless explicitly stated
+ * @ingroup geometry
*/
- static Rot2 relativeBearing(const Point2& d, boost::optional H=boost::none);
+ class Rot2: public Lie {
- /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
- static Rot2 atan2(double y, double x);
+ public:
+ /** get the dimension by the type */
+ static const size_t dimension = 1;
- /** return angle (RADIANS) */
- double theta() const { return ::atan2(s_,c_); }
+ private:
- /** return angle (DEGREES) */
- double degrees() const {
- const double degree = M_PI / 180;
- return theta() / degree;
- }
+ /** we store cos(theta) and sin(theta) */
+ double c_, s_;
- /** return cos */
- inline double c() const { return c_; }
+ /** private constructor from cos/sin */
+ inline Rot2(double c, double s) :
+ c_(c), s_(s) {
+ }
- /** return sin */
- inline double s() const { return s_; }
+ /** normalize to make sure cos and sin form unit vector */
+ Rot2& normalize();
- /** print */
- void print(const std::string& s = "theta") const;
+ public:
- /** equals with an tolerance */
- bool equals(const Rot2& R, double tol = 1e-9) const;
+ /** default constructor, zero rotation */
+ Rot2() :
+ c_(1.0), s_(0.0) {
+ }
- /** dimension of the variable - used to autodetect sizes */
- inline static size_t Dim() { return dimension; }
+ /// Constructor from angle in radians == exponential map at identity
+ Rot2(double theta) :
+ c_(cos(theta)), s_(sin(theta)) {
+ }
- /** Lie requirements */
+ /** "named constructors" */
- /** Dimensionality of the tangent space */
- inline size_t dim() const { return dimension; }
+ /// Named constructor from angle in radians
+ static Rot2 fromAngle(double theta) {
+ return Rot2(theta);
+ }
- /** Compose - make a new rotation by adding angles */
- inline Rot2 compose(const Rot2& R1,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if(H1) *H1 = eye(1);
- if(H2) *H2 = eye(1);
- return *this * R1;
- }
+ /// Named constructor from angle in degrees
+ static Rot2 fromDegrees(double theta) {
+ const double degree = M_PI / 180;
+ return fromAngle(theta * degree);
+ }
- /** Expmap around identity - create a rotation from an angle */
- static Rot2 Expmap(const Vector& v) {
- if (zero(v)) return (Rot2());
- else return Rot2::fromAngle(v(0));
- }
+ /// Named constructor from cos(theta),sin(theta) pair, will *not* normalize!
+ static Rot2 fromCosSin(double c, double s);
- /** Logmap around identity - return the angle of the rotation */
- static inline Vector Logmap(const Rot2& r) {
- return Vector_(1, r.theta());
- }
+ /**
+ * Named constructor with derivative
+ * Calculate relative bearing to a landmark in local coordinate frame
+ * @param point 2D location of landmark
+ * @param H optional reference for Jacobian
+ * @return 2D rotation \in SO(2)
+ */
+ static Rot2 relativeBearing(const Point2& d, boost::optional H =
+ boost::none);
- /** Binary expmap */
- inline Rot2 expmap(const Vector& v) const { return *this * Expmap(v); }
+ /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
+ static Rot2 atan2(double y, double x);
- /** Binary Logmap */
- inline Vector logmap(const Rot2& p2) const { return Logmap(between(p2));}
+ /** return angle (RADIANS) */
+ double theta() const {
+ return ::atan2(s_, c_);
+ }
- /** Between using the default implementation */
- inline Rot2 between(const Rot2& p2,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if(H1) *H1 = -eye(1);
- if(H2) *H2 = eye(1);
- return between_default(*this, p2);
- }
+ /** return angle (DEGREES) */
+ double degrees() const {
+ const double degree = M_PI / 180;
+ return theta() / degree;
+ }
- /** return 2*2 rotation matrix */
- Matrix matrix() const;
+ /** return cos */
+ inline double c() const {
+ return c_;
+ }
- /** return 2*2 transpose (inverse) rotation matrix */
- Matrix transpose() const;
+ /** return sin */
+ inline double s() const {
+ return s_;
+ }
- /** The inverse rotation - negative angle */
- Rot2 inverse() const { return Rot2(c_, -s_);}
+ /** print */
+ void print(const std::string& s = "theta") const;
- /** Compose - make a new rotation by adding angles */
- Rot2 operator*(const Rot2& R) const {
+ /** equals with an tolerance */
+ bool equals(const Rot2& R, double tol = 1e-9) const;
+
+ /** dimension of the variable - used to autodetect sizes */
+ inline static size_t Dim() {
+ return dimension;
+ }
+
+ /** Lie requirements */
+
+ /** Dimensionality of the tangent space */
+ inline size_t dim() const {
+ return dimension;
+ }
+
+ /** Compose - make a new rotation by adding angles */
+ inline Rot2 compose(const Rot2& R1, boost::optional H1 =
+ boost::none, boost::optional H2 = boost::none) const {
+ if (H1) *H1 = eye(1);
+ if (H2) *H2 = eye(1);
+ return *this * R1;
+ }
+
+ /** Expmap around identity - create a rotation from an angle */
+ static Rot2 Expmap(const Vector& v) {
+ if (zero(v))
+ return (Rot2());
+ else
+ return Rot2::fromAngle(v(0));
+ }
+
+ /** Logmap around identity - return the angle of the rotation */
+ static inline Vector Logmap(const Rot2& r) {
+ return Vector_(1, r.theta());
+ }
+
+ /** Binary expmap */
+ inline Rot2 expmap(const Vector& v) const {
+ return *this * Expmap(v);
+ }
+
+ /** Binary Logmap */
+ inline Vector logmap(const Rot2& p2) const {
+ return Logmap(between(p2));
+ }
+
+ /** Between using the default implementation */
+ inline Rot2 between(const Rot2& p2, boost::optional H1 =
+ boost::none, boost::optional H2 = boost::none) const {
+ if (H1) *H1 = -eye(1);
+ if (H2) *H2 = eye(1);
+ return between_default(*this, p2);
+ }
+
+ /** return 2*2 rotation matrix */
+ Matrix matrix() const;
+
+ /** return 2*2 transpose (inverse) rotation matrix */
+ Matrix transpose() const;
+
+ /** The inverse rotation - negative angle */
+ Rot2 inverse() const {
+ return Rot2(c_, -s_);
+ }
+
+ /** Compose - make a new rotation by adding angles */
+ Rot2 operator*(const Rot2& R) const {
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
- /**
- * rotate point from rotated coordinate frame to
- * world = R*p
- */
- Point2 rotate(const Point2& p, boost::optional H1 =
- boost::none, boost::optional H2 = boost::none) const;
+ /**
+ * rotate point from rotated coordinate frame to
+ * world = R*p
+ */
+ Point2 rotate(const Point2& p, boost::optional H1 = boost::none,
+ boost::optional H2 = boost::none) const;
- /** syntactic sugar for rotate */
- inline Point2 operator*(const Point2& p) const {return rotate(p);}
+ /** syntactic sugar for rotate */
+ inline Point2 operator*(const Point2& p) const {
+ return rotate(p);
+ }
- /**
- * rotate point from world to rotated
- * frame = R'*p
- */
- Point2 unrotate(const Point2& p, boost::optional H1 =
- boost::none, boost::optional H2 = boost::none) const;
+ /**
+ * rotate point from world to rotated
+ * frame = R'*p
+ */
+ Point2 unrotate(const Point2& p, boost::optional H1 = boost::none,
+ boost::optional H2 = boost::none) const;
- /**
- * Creates a unit vector as a Point2
- */
- inline Point2 unit() const { return Point2(c_, s_); }
+ /**
+ * Creates a unit vector as a Point2
+ */
+ inline Point2 unit() const {
+ return Point2(c_, s_);
+ }
- private:
- /** Serialization function */
- friend class boost::serialization::access;
- template
- void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & BOOST_SERIALIZATION_NVP(c_);
- ar & BOOST_SERIALIZATION_NVP(s_);
- }
+ private:
+ /** Serialization function */
+ friend class boost::serialization::access;
+ template
+ void serialize(ARCHIVE & ar, const unsigned int version) {
+ ar & BOOST_SERIALIZATION_NVP(c_);
+ ar & BOOST_SERIALIZATION_NVP(s_);
+ }
- }; // Rot2
+ };
} // gtsam
diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h
index 249e420a0..774e0c78a 100644
--- a/gtsam/geometry/concepts.h
+++ b/gtsam/geometry/concepts.h
@@ -13,6 +13,9 @@
#pragma once
+#include
+#include
+
namespace gtsam {
/**
diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp
index 02695c5ab..15a81d0f4 100644
--- a/gtsam/geometry/tests/testRot2.cpp
+++ b/gtsam/geometry/tests/testRot2.cpp
@@ -30,6 +30,8 @@ TEST( Rot2, constructors_and_angle)
{
double c=cos(0.1), s=sin(0.1);
DOUBLES_EQUAL(0.1,R.theta(),1e-9);
+ CHECK(assert_equal(R,Rot2(0.1)));
+ CHECK(assert_equal(R,Rot2::fromAngle(0.1)));
CHECK(assert_equal(R,Rot2::fromCosSin(c,s)));
CHECK(assert_equal(R,Rot2::atan2(s*5,c*5)));
}
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index 5296a092a..d17dd135e 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -16,9 +16,9 @@
#pragma once
-#include
-#include
#include
+#include
+#include
namespace gtsam {
@@ -58,12 +58,12 @@ namespace gtsam {
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose& pose, const Point& point,
boost::optional H1, boost::optional H2) const {
- Rot2 hx = pose.bearing(point, H1, H2);
+ Rot hx = pose.bearing(point, H1, H2);
return Rot::Logmap(z_.between(hx));
}
/** return the measured */
- inline const Rot2 measured() const {
+ inline const Rot measured() const {
return z_;
}
diff --git a/gtsam/slam/Landmark2.h b/gtsam/slam/Landmark2.h
new file mode 100644
index 000000000..f67cfed56
--- /dev/null
+++ b/gtsam/slam/Landmark2.h
@@ -0,0 +1,27 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Landmark2.h
+ * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include
+
+namespace gtsam {
+
+ typedef gtsam::Point2 Landmark2;
+
+}
+
diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am
index 42716f967..95c2f1488 100644
--- a/gtsam/slam/Makefile.am
+++ b/gtsam/slam/Makefile.am
@@ -50,6 +50,11 @@ check_PROGRAMS += tests/testPose2SLAM
sources += planarSLAM.cpp
check_PROGRAMS += tests/testPlanarSLAM
+# MATLAB Wrap headers for planarSLAM
+headers += Landmark2.h
+headers += PlanarSLAMGraph.h
+headers += PlanarSLAMValues.h
+
# 3D Pose constraints
sources += pose3SLAM.cpp
check_PROGRAMS += tests/testPose3SLAM
diff --git a/gtsam/slam/PlanarSLAMGraph.h b/gtsam/slam/PlanarSLAMGraph.h
new file mode 100644
index 000000000..bc6263997
--- /dev/null
+++ b/gtsam/slam/PlanarSLAMGraph.h
@@ -0,0 +1,29 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file PlanarSLAMGraph.h
+ * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+namespace gtsam {
+
+ typedef gtsam::planarSLAM::Graph PlanarSLAMGraph;
+
+}
+
diff --git a/gtsam/slam/PlanarSLAMValues.h b/gtsam/slam/PlanarSLAMValues.h
new file mode 100644
index 000000000..eba53b68a
--- /dev/null
+++ b/gtsam/slam/PlanarSLAMValues.h
@@ -0,0 +1,27 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file PlanarSLAMValues.h
+ * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include
+
+namespace gtsam {
+
+ typedef gtsam::planarSLAM::Values PlanarSLAMValues;
+
+}
+
diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index a27fcec0c..bfb6d482d 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -17,16 +17,17 @@
#pragma once
-#include
-#include
-#include
#include
#include
#include
#include
#include
+#include
+#include
#include
+#include
#include
+#include
// We use gtsam namespace for generally useful factors
namespace gtsam {
@@ -46,10 +47,27 @@ namespace gtsam {
/// Typedef for LieValues structure with PointKey type
typedef LieValues PointValues;
- /// Typedef for the TupleValues2 to use PoseKeys and PointKeys
- typedef TupleValues2 Values;
+ /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
+ struct Values: public TupleValues2 {
- /**
+ /// Default constructor
+ Values() {}
+
+ /// Copy constructor
+ Values(const TupleValues2& values) :
+ TupleValues2(values) {
+ }
+
+ // Convenience for MATLAB wrapper, which does not allow for identically named methods
+
+ /// insert a pose
+ void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
+
+ /// insert a point
+ void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
+ };
+
+ /**
* List of typedefs for factors
*/
@@ -96,6 +114,15 @@ namespace gtsam {
/// Creates a factor with a Rot2 between a PoseKey i and PointKey j for difference in rotation and location
void addBearingRange(const PoseKey& i, const PointKey& j,
const Rot2& z1, double z2, const SharedNoiseModel& model);
+
+ /// Optimize, mostly here for MATLAB
+ boost::shared_ptr optimize(const Values& initialEstimate) {
+ typedef NonlinearOptimizer Optimizer;
+// NonlinearOptimizationParameters::LAMBDA
+ boost::shared_ptr result(
+ new Values(*Optimizer::optimizeGN(*this, initialEstimate)));
+ return result;
+ }
};
/// Optimizer
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index fa8a1d65b..374e25584 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -210,6 +210,7 @@ void Module::matlab_code(const string& toolboxPath,
cls.matlab_methods(classPath,nameSpace);
// add lines to make m-file
+ ofs << "%% " << cls.name << endl;
ofs << "cd(toolboxpath)" << endl;
cls.matlab_make_fragment(ofs, toolboxPath, mexFlags);
}
diff --git a/wrap/README b/wrap/README
index 2556c179b..6996e3649 100644
--- a/wrap/README
+++ b/wrap/README
@@ -24,4 +24,7 @@ METHOD (AND CONSTRUCTOR) ARGUMENTS
generation of the correct conversion routines unwrap and unwrap
- passing classes as arguments works, provided they are passed by reference.
This triggers a call to unwrap_shared_ptr
+- GTSAM specific: keys will be cast automatically from strings via GTSAM_magic. This
+ allows us to not have to declare all key types in MATLAB. Just replace key arguments with
+ the (non-const, non-reference) string type
\ No newline at end of file