diff --git a/.cproject b/.cproject
index 221a18374..85c3942c4 100644
--- a/.cproject
+++ b/.cproject
@@ -543,6 +543,22 @@
true
true
+
+make
+
+testControlConfig.run
+true
+true
+true
+
+
+make
+
+testControlPoint.run
+true
+true
+true
+
make
install
diff --git a/cpp/ControlConfig.cpp b/cpp/ControlConfig.cpp
new file mode 100644
index 000000000..9b3a3d125
--- /dev/null
+++ b/cpp/ControlConfig.cpp
@@ -0,0 +1,118 @@
+/**
+ * @file ControlConfig.cpp
+ * @brief Implementation of ControlConfig
+ * @author Alex Cunningham
+ */
+
+#include
+#include
+#include
+#include
+#include "ControlConfig.h"
+
+using namespace std;
+using namespace gtsam;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+// convert to strings
+template
+string toStr(const T& t) {
+ ostringstream oss;
+ oss << t;
+ return oss.str();
+}
+
+/* *************************************************************** */
+void ControlConfig::print(const std::string& name) const {
+ cout << "Config: " << name << endl;
+ string agent; path p;
+ FOREACH_PAIR(agent, p, paths_) {
+ cout << "Agent: " << agent << "\n";
+ int i = 0;
+ BOOST_FOREACH(ControlPoint pt, p) {
+ ostringstream oss;
+ oss << "Point: " << i++;
+ pt.print(oss.str());
+ }
+ cout << endl;
+ }
+}
+
+/* *************************************************************** */
+bool ControlConfig::equals(const ControlConfig& expected, double tol) const {
+ if (paths_.size() != expected.paths_.size()) return false;
+ string j; path pa;
+ FOREACH_PAIR(j, pa, paths_) {
+ if (!expected.involvesAgent(j))
+ return false;
+ path pb = expected.getPath(j);
+ if (pa.size() != pb.size())
+ return false;
+ for (int i=0; isecond;
+ } else {
+ throw invalid_argument("Attempting to access path that does not exist");
+ }
+}
+
+/* *************************************************************** */
+bool ControlConfig::involvesAgent(const std::string& agentID) const {
+ return paths_.find(agentID) != paths_.end();
+}
+
+/* *************************************************************** */
+void ControlConfig::clearAgent(const std::string& agentID) {
+ const_iterator it = paths_.find(agentID);
+ if (it != paths_.end()) {
+ path &p = paths_[agentID];
+ p.clear();
+ } else {
+ throw invalid_argument("Attempting to clear agent that is not present");
+ }
+}
+
+/* *************************************************************** */
+ControlConfig ControlConfig::exmap(const VectorConfig & delta) const {
+ ControlConfig newConfig; string agent; path p;
+ FOREACH_PAIR(agent, p, paths_) {
+ newConfig.addAgent(agent);
+ for (size_t i=0; i
+#include
+#include "Testable.h"
+#include "ControlPoint.h"
+#include "VectorConfig.h"
+
+namespace gtsam {
+
+/**
+ * Class for configs of 2D agent motion models that make up trajectories
+ * Provides a map of labeled robot poses, and means to access groups, such
+ * as the trajectory of a particular robot or obstacle.
+ */
+class ControlConfig : public Testable {
+public:
+ /** an individual path object for an agent */
+ typedef std::vector path;
+ typedef std::map::const_iterator const_iterator;
+
+private:
+ /** main storage for points */
+ std::map paths_;
+
+public:
+ /** Basic Default Constructors */
+ ControlConfig() {}
+ ControlConfig(const ControlConfig& cfg) : paths_(cfg.paths_) {}
+
+ /** Default destructor */
+ virtual ~ControlConfig() {}
+
+ /** Standard print function with optional label */
+ virtual void print(const std::string& name="") const;
+
+ /** Equality up to a tolerance */
+ virtual bool equals(const ControlConfig& expected, double tol=1e-9) const;
+
+ /** Add a delta configuration to the config */
+ ControlConfig exmap(const VectorConfig & delta) const;
+
+ /** number of agents */
+ size_t size() const { return paths_.size(); }
+
+ /**
+ * Adds an agent to the config
+ * @param name is the name of the agent used for lookup
+ */
+ void addAgent(const std::string& name);
+
+ /**
+ * Adds a point to a robot's trajectory,
+ * note that ordering is handled internally
+ * @param name is the name of the robot
+ * @param state is the ControlPoint to add
+ */
+ void addPoint(const std::string& name, const ControlPoint& state);
+
+ /**
+ * returns the path of a particular robot
+ */
+ path getPath(const std::string& agentID) const;
+
+ /**
+ * Returns true if agent is in the config
+ */
+ bool involvesAgent(const std::string& agentID) const;
+
+ // clearing
+ void clear() { paths_.clear(); }
+ void clearAgent(const std::string& agentID);
+};
+} // \namespace gtsam
+
diff --git a/cpp/ControlPoint.cpp b/cpp/ControlPoint.cpp
new file mode 100644
index 000000000..ded9a5bb8
--- /dev/null
+++ b/cpp/ControlPoint.cpp
@@ -0,0 +1,48 @@
+/**
+ * @file ControlPoint.cpp
+ * @brief Implementation of ControlPoint
+ * @author Alex Cunningham
+ */
+
+#include
+#include "ControlPoint.h"
+
+using namespace std;
+using namespace gtsam;
+
+ControlPoint::ControlPoint()
+: time_(0.0)
+{ // Note that default pose2 constructors are at (0,0,0)
+}
+
+ControlPoint::ControlPoint(const Pose2& pos, const Pose2& vel, double time)
+: pos_(pos), vel_(vel), time_(time)
+{
+}
+
+ControlPoint::ControlPoint(double posx, double posy, double posr,
+ double velx, double vely, double velr, double time)
+: pos_(posx, posy, posr), vel_(velx, vely, velr), time_(time)
+{
+}
+
+void ControlPoint::print(const std::string& name) const {
+ cout << "ControlPoint: " << name << " at time = " << time_ << endl;
+ pos_.print("Position");
+ vel_.print("Velocity");
+}
+
+bool ControlPoint::equals(const ControlPoint& pt, double tol) const {
+ bool time_equal = abs(time_-pt.time()) < tol;
+ return time_equal && pos_.equals(pt.pos()) && vel_.equals(pt.vel());
+}
+
+ControlPoint ControlPoint::exmap(const Vector& delta) {
+ //TODO: bound the angle for position to -pi < theta < pi
+ Pose2 newPos(pos_.x()+delta(0), pos_.y()+delta(1), pos_.theta()+delta(2));
+ Pose2 newVel(vel_.x()+delta(3), vel_.y()+delta(4), vel_.theta()+delta(5));
+ double newTime = time_ + delta(6);
+ return ControlPoint(newPos, newVel, newTime);
+}
+
+
diff --git a/cpp/ControlPoint.h b/cpp/ControlPoint.h
new file mode 100644
index 000000000..0708edd82
--- /dev/null
+++ b/cpp/ControlPoint.h
@@ -0,0 +1,70 @@
+/*
+ * @file ControlPoint.h
+ * @brief Class with a point in a Position-Velocity model at a given time for 2D robots
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include "Pose2.h"
+#include "Testable.h"
+
+namespace gtsam {
+
+/**
+ * This class stores a single point in time using a model
+ * with position and velocity, as well as a time stamp, and
+ * is designed for use with robot control applications.
+ */
+class ControlPoint : public Testable {
+private:
+ // position model
+ Pose2 pos_;
+
+ // velocity model
+ Pose2 vel_;
+
+ // timestamp for this observation
+ double time_;
+
+public:
+ /** default contructor: stationary point at origin at zero time*/
+ ControlPoint();
+
+ /** full constructor */
+ ControlPoint(const Pose2& pos, const Pose2& vel, double time);
+
+ /** manual constructor - specify each Pose2 in full */
+ ControlPoint(double posx, double posy, double posr,
+ double velx, double vely, double velr, double time);
+
+ /** default destructor */
+ virtual ~ControlPoint() {}
+
+ /** Standard print function with optional label */
+ virtual void print(const std::string& name="") const;
+
+ /** Equality up to a tolerance */
+ virtual bool equals(const ControlPoint& expected, double tol=1e-9) const;
+
+ /* Access functions */
+ Pose2 pos() const { return pos_; }
+ Pose2 vel() const { return vel_; }
+ double time() const { return time_; }
+
+ /**
+ * Exmap function to add a delta configuration to the point
+ * NOTE: in handling rotation, the position will have its
+ * range bounded to -pi < r < pi, but the velocity
+ * can be larger than 2pi, as this would represent that
+ * the angular velocity will do more than a full rotation
+ * in a time step.
+ */
+ ControlPoint exmap(const Vector& delta);
+};
+ // comparison
+
+}
+
+
+
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 160888a8d..2da91073f 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -160,6 +160,15 @@ testSimulated3D_SOURCES = testSimulated3D.cpp
testSimulated3D_LDADD = libgtsam.la
check_PROGRAMS += testSimulated3D
+# Robot Control example system
+sources += ControlConfig.cpp ControlPoint.cpp
+#headers += ControlConfig.h ControlPoint.h
+check_PROGRAMS += testControlConfig testControlPoint
+testControlConfig_SOURCES = testControlConfig.cpp
+testControlConfig_LDADD = libgtsam.la
+testControlPoint_SOURCES = testControlPoint.cpp
+testControlPoint_LDADD = libgtsam.la
+
# Cameras
sources += CalibratedCamera.cpp SimpleCamera.cpp
check_PROGRAMS += testCalibratedCamera testSimpleCamera
diff --git a/cpp/testControlConfig.cpp b/cpp/testControlConfig.cpp
new file mode 100644
index 000000000..c96168012
--- /dev/null
+++ b/cpp/testControlConfig.cpp
@@ -0,0 +1,111 @@
+/**
+ * @file testControlConfig.cpp
+ * @brief Test for configuration using 2D control inputs on a PV model
+ * @author Alex Cunningham
+ */
+
+#include
+#include
+#include
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( ControlConfig, basic ) {
+
+ // create a config
+ ControlConfig config;
+
+ // add a robot to the config
+ string r1 = "R1", r2 = "R2";
+ config.addAgent(r1);
+ config.addAgent(r2);
+
+ // add some states for each of the robots
+ ControlPoint s1,
+ s2(Pose2(1.0, 1.0, 1.0), Pose2(), 2.0),
+ s3(Pose2(1.0, 1.0, 1.0), Pose2(), 3.0);
+ config.addPoint(r1, s1);
+ config.addPoint(r1, s2);
+ config.addPoint(r1, s3);
+
+ // get the path back out again
+ ControlConfig::path act1 = config.getPath(r1);
+ CHECK(act1.size() == 3);
+ CHECK(assert_equal(act1.at(0), s1));
+ CHECK(assert_equal(act1.at(1), s2));
+ CHECK(assert_equal(act1.at(2), s3));
+
+ // check size
+ CHECK(config.size() == 2);
+}
+
+/* ************************************************************************* */
+TEST ( ControlConfig, equals ) {
+ ControlConfig cfg1, cfg2, cfg3;
+ cfg1.addAgent("r1");
+ cfg2.addAgent("r1");
+ cfg3.addAgent("r2");
+
+ CHECK(assert_equal(cfg1, cfg2));
+ CHECK(!cfg1.equals(cfg3));
+
+ ControlPoint s1, s2(Pose2(1.0, 1.0, 1.0), Pose2(), 2.0);
+ cfg1.addPoint("r1", s1);
+ cfg2.addPoint("r1", s2);
+
+ CHECK(!cfg1.equals(cfg2));
+}
+
+/* ************************************************************************* */
+TEST ( ControlConfig, exmap ) {
+ // create a config with two agents and some trajectories
+ ControlConfig config;
+ ControlPoint s1,
+ s2(Pose2(1.0, 1.0, 1.0), Pose2(), 1.0),
+ s3(Pose2(2.0, 2.0, 2.0), Pose2(0.1, 0.2, 0.3), 2.0),
+ s4(Pose2(1.0, 2.0, 1.0), Pose2(), 0.0),
+ s5(Pose2(3.0, 4.0, 3.0), Pose2(0.4, 0.5, 0.6), 1.5);
+
+
+ config.addAgent("r1");
+ config.addPoint("r1", s1);
+ config.addPoint("r1", s2);
+ config.addPoint("r1", s3);
+ config.addAgent("r2");
+ config.addPoint("r2", s4);
+ config.addPoint("r2", s5);
+
+ // create a delta config
+ VectorConfig delta;
+ Vector d1 = repeat(7, 0.1);
+ Vector d2 = repeat(7, 0.2);
+ Vector d3 = repeat(7, 0.3);
+ Vector dother = repeat(7, 100.0);
+ delta.insert("r1_0", d1);
+ delta.insert("r1_1", d2);
+ delta.insert("r1_2", d3);
+ delta.insert("r2_0", d1);
+ delta.insert("r2_1", d2);
+ delta.insert("penguin", dother);
+
+
+ ControlConfig actual = config.exmap(delta);
+
+ // Verify
+ ControlConfig expected;
+ expected.addAgent("r1");
+ expected.addPoint("r1", s1.exmap(d1));
+ expected.addPoint("r1", s2.exmap(d2));
+ expected.addPoint("r1", s3.exmap(d3));
+ expected.addAgent("r2");
+ expected.addPoint("r2", s4.exmap(d1));
+ expected.addPoint("r2", s5.exmap(d2));
+
+ CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/cpp/testControlPoint.cpp b/cpp/testControlPoint.cpp
new file mode 100644
index 000000000..00724b34b
--- /dev/null
+++ b/cpp/testControlPoint.cpp
@@ -0,0 +1,72 @@
+/**
+ * @file testControlPoint.cpp
+ * @brief A single point in time on a trajectory
+ * @author Alex Cunningham
+ */
+
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+using namespace gtsam;
+
+TEST ( ControlPoint, constructors ) {
+
+ // make a default point, stationary at time zero at the origin
+ ControlPoint pt1;
+ CHECK(assert_equal(pt1.pos(), Pose2()));
+ CHECK(assert_equal(pt1.vel(), Pose2()));
+ CHECK(pt1.time() < 1e-9); // check for zero time
+
+ // make a point in same place to test constructors
+ ControlPoint pt2(Pose2(), Pose2(), 0.0);
+ CHECK(assert_equal(pt2.pos(), Pose2()));
+ CHECK(assert_equal(pt2.vel(), Pose2()));
+ CHECK(pt2.time() < 1e-9); // check for zero time
+
+ // check equality
+ CHECK(assert_equal(pt2, pt1));
+
+ // make a specific point
+ Pose2 pos(1.0, 2.0, 3.0);
+ Pose2 vel(0.1, 0.2, 0.3);
+ double time = 0.5;
+ ControlPoint pt3(pos, vel, time);
+ CHECK(assert_equal(pt3.pos(), pos));
+ CHECK(assert_equal(pt3.vel(), vel));
+ CHECK(fabs(pt3.time()-time) < 1e-9);
+
+ // use manual constructor
+ double posx=1.0, posy=2.0, posr=3.0;
+ double velx=0.1, vely=0.2, velr=0.3;
+ ControlPoint pt4(posx, posy, posr, velx, vely, velr, time);
+ CHECK(assert_equal(pt3, pt4));
+}
+
+TEST ( ControlPoint, exmap ) {
+ // add a delta to an existing point
+ Pose2 pos(1.0, 2.0, 3.0);
+ Pose2 vel(0.1, 0.2, 0.3);
+ double time = 0.5;
+ ControlPoint pt(pos, vel, time);
+
+ // ensure that zero vector doesn't change the point
+ ControlPoint act1 = pt.exmap(zero(7));
+ CHECK(assert_equal(pt, act1));
+
+ // add a real delta
+ Vector delta1 = Vector_(7, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7);
+ ControlPoint act2 = pt.exmap(delta1);
+ Pose2 pos_exp(1.1, 2.2, 3.3);
+ Pose2 vel_exp(0.5, 0.7, 0.9);
+ double time_exp = 1.2;
+ ControlPoint pt_exp(pos_exp, vel_exp, time_exp);
+ CHECK(assert_equal(act2, pt_exp));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */