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

91
gtsam_unstable/geometry/Event.h
View File

@ -20,42 +20,43 @@
#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
double time_; ///< Time event was generated
Point3 location_; ///< Location at time event was generated
public:
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_;}
double time() const { return time_; }
Point3 location() const { return location_; }
// TODO 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();
// 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;
return location_.z();
}
@ -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,28 +75,37 @@ 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!
}
};
// Define GTSAM traits
template<>
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

42
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;
@ -30,56 +32,59 @@ using namespace gtsam;
static const double ms = 1e-3;
static const double cm = 1e-2;
typedef Eigen::Matrix<double, 1, 1> Vector1;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0,0,0);
static const Point3 microphoneAt0(0, 0, 0);
static const double kSpeedOfSound = 340;
static const TimeOfArrival kToa(kSpeedOfSound);
//*****************************************************************************
TEST( Event, Constructor ) {
TEST(Event, Constructor) {
const double t = 0;
Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
}
//*****************************************************************************
TEST( Event, Toa1 ) {
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);
TEST(Event, Toa2) {
double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
}
//*************************************************************************
TEST (Event, Derivatives) {
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));
}
//*****************************************************************************
TEST( Event, Expression ) {
TEST(Event, Expression) {
Key key = 12;
Expression<Event> event_(key);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
Expression<double> expression(&Event::toa, event_, knownMicrophone_);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
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);
}
//*****************************************************************************

44
gtsam_unstable/slam/TOAFactor.h
View File

@ -17,33 +17,47 @@
* @date December 2014
*/
#pragma once
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam_unstable/geometry/Event.h>
namespace gtsam {
/// A "Time of Arrival" factor - so little code seems hardly worth it :-)
class TOAFactor: public ExpressionFactor<double> {
class TOAFactor : public ExpressionFactor<double> {
typedef Expression<double> Double_;
public:
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

49
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;
@ -43,21 +44,18 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0,0,0);
static const Point3 microphoneAt0(0, 0, 0);
//*****************************************************************************
TEST( TOAFactor, NewWay ) {
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 ) {
TEST(TOAFactor, WholeEnchilada) {
static const bool verbose = false;
// Create microphones
@ -68,7 +66,7 @@ TEST( TOAFactor, WholeEnchilada ) {
microphones.push_back(Point3(403 * cm, 403 * cm, height));
microphones.push_back(Point3(0, 403 * cm, 2 * height));
EXPECT_LONGS_EQUAL(4, microphones.size());
// microphones.push_back(Point3(200 * cm, 200 * cm, height));
// microphones.push_back(Point3(200 * cm, 200 * cm, height));
// Create a ground truth point
const double timeOfEvent = 0;
@ -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,40 +85,35 @@ 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;
//Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
// Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
Vector4 delta;
delta << 0.1, 0.1, -0.1, 0.1;
Event estimatedEvent = groundTruthEvent.retract(delta);
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