diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 649abc455..039c963d6 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -50,6 +50,9 @@ public: time_(t), location_(x, y, z) { } + double time() const { return time_;} + Point3 location() const { return location_;} + /** print with optional string */ void print(const std::string& s = "") const { std::cout << s << "time = " << time_; diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 13a4b0996..8d98f8db5 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -177,7 +178,7 @@ TEST( TOAFactor, RealExperiment1 ) { data[i++] << 7.8286, 7.8286, 7.8302, 7.8353; // Create unknowns and initial estimate - Event nullEvent; + Event nullEvent(3, 403 / 2 * cm, 403 / 2 * cm, (212 - 45) * cm); Values initialEstimate; vector > eventExpressions; for (size_t j = 0; j < 15; j++) { @@ -209,7 +210,14 @@ TEST( TOAFactor, RealExperiment1 ) { LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); if (verbose) - result.print("Final Result:\n"); + for (size_t j = 0; j < 15; j++) { + Event event = result.at(j); + double t = event.time(); + Point3 p = event.location(); + cout + << boost::format("t(%1%) = %2%;\tlocation(%1%,:) = [%3%, %4%, %5%];") + % (j + 1) % t % p.x() % p.y() % p.z() << endl; + } } //*****************************************************************************