diff --git a/.cproject b/.cproject
index 9c03c5b7d..bb38b29b1 100644
--- a/.cproject
+++ b/.cproject
@@ -848,6 +848,22 @@
true
true
+
+ make
+ -j4
+ testSmartStereoProjectionPoseFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testTOAFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -2393,6 +2409,46 @@
true
true
+
+ make
+ -j4
+ testType.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testArgument.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testReturnValue.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testTemplate.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testGlobalFunction.run
+ true
+ true
+ true
+
make
-j5
@@ -2902,6 +2958,14 @@
true
true
+
+ make
+ -j4
+ testRangeFactor.run
+ true
+ true
+ true
+
make
-j2
diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp
new file mode 100644
index 000000000..7f4bbbb38
--- /dev/null
+++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp
@@ -0,0 +1,112 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testTOAFactor.cpp
+ * @brief Unit tests for "Time of Arrival" factor
+ * @author Frank Dellaert
+ * @author Jay Chakravarty
+ * @date December 2014
+ */
+
+#include
+
+namespace gtsam {
+
+/// A space-time event
+class Event {
+
+ double time_; ///< Time event was generated
+ Point3 location_; ///< Location at time event was generated
+
+public:
+
+ /// Speed of sound
+ static const double Speed;
+
+ /// Constructor
+ Event(double t, double x, double y, double z) :
+ time_(t), location_(x, y, z) {
+ }
+
+ /// Time of arrival to given microphone
+ double toa(const Point3& microphone) {
+ return time_ + location_.distance(microphone) / Speed;
+ }
+};
+
+const double Event::Speed = 330;
+
+} //\ namespace gtsam
+
+#include
+#include
+#include
+
+using namespace std;
+using namespace gtsam;
+
+// Create a noise model for the TOA error
+static const double ms = 1e-3, cm = 1e-2;
+typedef Eigen::Matrix Vector1;
+static SharedNoiseModel model(noiseModel::Diagonal::Sigmas(Vector1(5. * ms)));
+
+//*****************************************************************************
+TEST( Event, Constructor ) {
+ const double t = 0;
+ Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
+}
+
+//*****************************************************************************
+TEST( TOA, Toa1 ) {
+ Point3 microphone;
+ Event event(0, 1, 0, 0);
+ double expected = 1 / Event::Speed;
+ EXPECT_DOUBLES_EQUAL(expected, event.toa(microphone), 1e-9);
+}
+
+//*****************************************************************************
+TEST( TOA, Toa2 ) {
+ Point3 microphone;
+ double timeOfEvent = 25;
+ Event event(timeOfEvent, 1, 0, 0);
+ double expectedTOA = timeOfEvent + 1 / Event::Speed;
+ EXPECT_DOUBLES_EQUAL(expectedTOA, event.toa(microphone), 1e-9);
+}
+
+//*****************************************************************************
+TEST( TOAFactor, WholeEnchilada ) {
+
+ // Create microphones
+ vector microphones;
+ microphones.push_back(Point3(0, 0, 0));
+ microphones.push_back(Point3(403 * cm, 0, 0));
+ microphones.push_back(Point3(403 * cm, 403 * cm, 0));
+ microphones.push_back(Point3(0, 403 * cm, 0));
+ EXPECT_LONGS_EQUAL(4, microphones.size());
+
+ // Create a ground truth point
+ const double timeOfEvent = 0;
+ Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
+
+ // Simulate measurements
+ vector measurements(4);
+ for (size_t i = 0; i < 4; i++)
+ measurements[i] = event.toa(microphones[i]);
+}
+
+//*****************************************************************************
+int main() {
+ TestResult tr;
+ return TestRegistry::runAllTests(tr);
+}
+//*****************************************************************************
+