12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Transitioned toa method to a functor

release/4.3a0
Frank Dellaert 2020-03-17 14:34:11 -04:00
parent 16dbf27375
commit 2087075ee7
5 changed files with 125 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
gtsam_unstable/geometry/Event.h
View File

@ -20,15 +20,21 @@
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam_unstable/dllexport.h>
#include <cmath>
#include <iosfwd>
#include <gtsam_unstable/dllexport.h>
#include <string>
namespace gtsam {
/// A space-time event
/**
* A space-time event models an event that happens at a certain 3D location, at
* a certain time. One use for it is in sound-based or UWB-ranging tracking or
* SLAM, where we have "time of arrival" measurements at a set of sensors. The
* TOA functor below provides a measurement function for those applications.
*/
class Event {
double time_; ///< Time event was generated
Point3 location_; ///< Location at time event was generated
@ -36,24 +42,19 @@ public:
enum { dimension = 4 };
/// Default Constructor
Event() :
time_(0), location_(0,0,0) {
}
Event() : time_(0), location_(0, 0, 0) {}
/// Constructor from time and location
Event(double t, const Point3& p) :
time_(t), location_(p) {
}
Event(double t, const Point3& p) : time_(t), location_(p) {}
/// Constructor with doubles
Event(double t, double x, double y, double z) :
time_(t), location_(x, y, z) {
}
Event(double t, double x, double y, double z)
: time_(t), location_(x, y, z) {}
double time() const { return time_; }
Point3 location() const { return location_; }
// TODO we really have to think of a better way to do linear arguments
// TODO(frank) we really have to think of a better way to do linear arguments
double height(OptionalJacobian<1, 4> H = boost::none) const {
static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
if (H) *H = JacobianZ;
@ -64,7 +65,8 @@ public:
GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const;
/** equals with an tolerance */
GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const;
GTSAM_UNSTABLE_EXPORT bool equals(const Event& other,
double tol = 1e-9) const;
/// Updates a with tangent space delta
inline Event retract(const Vector4& v) const {
@ -73,23 +75,7 @@ public:
/// Returns inverse retraction
inline Vector4 localCoordinates(const Event& q) const {
return Vector4::Zero(); // TODO
}
/// Time of arrival to given microphone
double toa(const Point3& microphone, //
OptionalJacobian<1, 4> H1 = boost::none, //
OptionalJacobian<1, 3> H2 = boost::none) const {
static const double Speed = 330;
Matrix13 D1, D2;
double distance = gtsam::distance3(location_, microphone, D1, D2);
if (H1)
// derivative of toa with respect to event
*H1 << 1.0, D1 / Speed;
if (H2)
// derivative of toa with respect to microphone location
*H2 << D2 / Speed;
return time_ + distance / Speed;
return Vector4::Zero(); // TODO(frank) implement!
}
};
@ -97,4 +83,29 @@ public:
template <>
struct traits<Event> : internal::Manifold<Event> {};
} //\ namespace gtsam
/// Time of arrival to given sensor
class TimeOfArrival {
const double speed_; ///< signal speed
public:
typedef double result_type;
/// Constructor with optional speed of signal, in m/sec
explicit TimeOfArrival(double speed = 330) : speed_(speed) {}
/// Calculate time of arrival
double operator()(const Event& event, const Point3& sensor, //
OptionalJacobian<1, 4> H1 = boost::none, //
OptionalJacobian<1, 3> H2 = boost::none) const {
Matrix13 D1, D2;
double distance = gtsam::distance3(event.location(), sensor, D1, D2);
if (H1)
// derivative of toa with respect to event
*H1 << 1.0, D1 / speed_;
if (H2)
// derivative of toa with respect to sensor location
*H2 << D2 / speed_;
return event.time() + distance / speed_;
}
};
} // namespace gtsam

26
gtsam_unstable/geometry/tests/testEvent.cpp
View File

@ -17,10 +17,12 @@
* @date December 2014
*/
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam_unstable/geometry/Event.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
@ -36,6 +38,9 @@ static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0, 0, 0);
static const double kSpeedOfSound = 340;
static const TimeOfArrival kToa(kSpeedOfSound);
//*****************************************************************************
TEST(Event, Constructor) {
const double t = 0;
@ -45,27 +50,27 @@ TEST( Event, Constructor ) {
//*****************************************************************************
TEST(Event, Toa1) {
Event event(0, 1, 0, 0);
double expected = 1. / 330;
EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
double expected = 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9);
}
//*****************************************************************************
TEST(Event, Toa2) {
double expectedTOA = timeOfEvent + 1. / 330;
EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
}
//*************************************************************************
TEST(Event, Derivatives) {
Matrix14 actualH1;
Matrix13 actualH2;
exampleEvent.toa(microphoneAt0, actualH1, actualH2);
kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
Matrix expectedH1 = numericalDerivative11<double, Event>(
boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
exampleEvent);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
Matrix expectedH2 = numericalDerivative11<double, Point3>(
boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none),
boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
microphoneAt0);
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}
@ -75,11 +80,11 @@ TEST( Event, Expression ) {
Key key = 12;
Expression<Event> event_(key);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
Expression<double> expression(&Event::toa, event_, knownMicrophone_);
Expression<double> expression(kToa, event_, knownMicrophone_);
Values values;
values.insert(key, exampleEvent);
double expectedTOA = timeOfEvent + 1. / 330;
double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
}
@ -97,4 +102,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
//*****************************************************************************

40
gtsam_unstable/slam/TOAFactor.h
View File

@ -17,6 +17,8 @@
* @date December 2014
*/
#pragma once
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam_unstable/geometry/Event.h>
@ -24,26 +26,38 @@ namespace gtsam {
/// A "Time of Arrival" factor - so little code seems hardly worth it :-)
class TOAFactor : public ExpressionFactor<double> {
typedef Expression<double> Double_;
public:
/**
* Constructor
* @param some expression yielding an event
* @param microphone_ expression yielding a microphone location
* @param toaMeasurement time of arrival at microphone
* Most genral constructor with two expressions
* @param eventExpression expression yielding an event
* @param sensorExpression expression yielding a sensor location
* @param toaMeasurement time of arrival at sensor
* @param model noise model
* @param toa optional time of arrival functor
*/
TOAFactor(const Expression<Event>& eventExpression,
const Expression<Point3>& microphone_, double toaMeasurement,
const SharedNoiseModel& model) :
ExpressionFactor<double>(model, toaMeasurement,
Double_(&Event::toa, eventExpression, microphone_)) {
}
const Expression<Point3>& sensorExpression, double toaMeasurement,
const SharedNoiseModel& model,
const TimeOfArrival& toa = TimeOfArrival())
: ExpressionFactor<double>(
model, toaMeasurement,
Double_(toa, eventExpression, sensorExpression)) {}
/**
* Constructor with fixed sensor
* @param eventExpression expression yielding an event
* @param sensor a known sensor location
* @param toaMeasurement time of arrival at sensor
* @param model noise model
* @param toa optional time of arrival functor
*/
TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
double toaMeasurement, const SharedNoiseModel& model,
const TimeOfArrival& toa = TimeOfArrival())
: TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
model, toa) {}
};
} //\ namespace gtsam
} // namespace gtsam

39
gtsam_unstable/slam/tests/testTOAFactor.cpp
View File

@ -17,15 +17,16 @@
* @date December 2014
*/
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam_unstable/slam/TOAFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
using namespace std;
using namespace gtsam;
@ -49,15 +50,12 @@ static const Point3 microphoneAt0(0,0,0);
TEST(TOAFactor, NewWay) {
Key key = 12;
Event_ eventExpression(key);
Point3_ microphoneConstant(microphoneAt0); // constant expression
double measurement = 7;
Double_ expression(&Event::toa, eventExpression, microphoneConstant);
ExpressionFactor<double> factor(model, measurement, expression);
TOAFactor factor(eventExpression, microphoneAt0, measurement, model);
}
//*****************************************************************************
TEST(TOAFactor, WholeEnchilada) {
static const bool verbose = false;
// Create microphones
@ -77,8 +75,9 @@ TEST( TOAFactor, WholeEnchilada ) {
// Simulate simulatedTOA
size_t K = microphones.size();
vector<double> simulatedTOA(K);
TimeOfArrival toa;
for (size_t i = 0; i < K; i++) {
simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
simulatedTOA[i] = toa(groundTruthEvent, microphones[i]);
if (verbose) {
cout << "mic" << i << " = " << microphones[i] << endl;
cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
@ -86,18 +85,16 @@ TEST( TOAFactor, WholeEnchilada ) {
}
// Now, estimate using non-linear optimization
ExpressionFactorGraph graph;
NonlinearFactorGraph graph;
Key key = 12;
Event_ eventExpression(key);
for (size_t i = 0; i < K; i++) {
Point3_ microphone_i(microphones[i]); // constant expression
Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
graph.emplace_shared<TOAFactor>(eventExpression, microphones[i],
simulatedTOA[i], model);
}
/// Print the graph
if (verbose)
GTSAM_PRINT(graph);
if (verbose) GTSAM_PRINT(graph);
// Create initial estimate
Values initialEstimate;
@ -108,18 +105,15 @@ TEST( TOAFactor, WholeEnchilada ) {
initialEstimate.insert(key, estimatedEvent);
// Print
if (verbose)
initialEstimate.print("Initial Estimate:\n");
if (verbose) initialEstimate.print("Initial Estimate:\n");
// Optimize using Levenberg-Marquardt optimization.
LevenbergMarquardtParams params;
params.setAbsoluteErrorTol(1e-10);
if (verbose)
params.setVerbosity("ERROR");
if (verbose) params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
if (verbose)
result.print("Final Result:\n");
if (verbose) result.print("Final Result:\n");
EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
}
@ -129,4 +123,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
//*****************************************************************************

1
wrap/python/pybind11 Submodule

@ -0,0 +1 @@
Subproject commit b3bf248eec9cad8260753c982e1ae6cb72fff470