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

Refactor TOAFactor and test

release/4.3a0
Frank Dellaert 2020-03-18 15:44:33 -04:00
parent 2087075ee7
commit f3865539c6
2 changed files with 19 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam_unstable/slam/TOAFactor.h
View File

@ -30,20 +30,19 @@ class TOAFactor : public ExpressionFactor<double> {
public: public:
/** /**
* Most genral constructor with two expressions * Most general constructor with two expressions
* @param eventExpression expression yielding an event * @param eventExpression expression yielding an event
* @param sensorExpression expression yielding a sensor location * @param sensorExpression expression yielding a sensor location
* @param toaMeasurement time of arrival at sensor * @param toaMeasurement time of arrival at sensor
* @param model noise model * @param model noise model
* @param toa optional time of arrival functor * @param speed optional speed of signal, in m/sec
*/ */
TOAFactor(const Expression<Event>& eventExpression, TOAFactor(const Expression<Event>& eventExpression,
const Expression<Point3>& sensorExpression, double toaMeasurement, const Expression<Point3>& sensorExpression, double toaMeasurement,
const SharedNoiseModel& model, const SharedNoiseModel& model, double speed = 330)
const TimeOfArrival& toa = TimeOfArrival())
: ExpressionFactor<double>( : ExpressionFactor<double>(
model, toaMeasurement, model, toaMeasurement,
Double_(toa, eventExpression, sensorExpression)) {} Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}
/** /**
* Constructor with fixed sensor * Constructor with fixed sensor
@ -55,9 +54,9 @@ class TOAFactor : public ExpressionFactor<double> {
*/ */
TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor, TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
double toaMeasurement, const SharedNoiseModel& model, double toaMeasurement, const SharedNoiseModel& model,
const TimeOfArrival& toa = TimeOfArrival()) double speed = 330)
: TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement, : TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
model, toa) {} model, speed) {}
}; };
} // namespace gtsam } // namespace gtsam

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

@ -44,58 +44,46 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
static const double timeOfEvent = 25; static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0); static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0, 0, 0); static const Point3 sensorAt0(0, 0, 0);
//***************************************************************************** //*****************************************************************************
TEST(TOAFactor, NewWay) { TEST(TOAFactor, NewWay) {
Key key = 12; Key key = 12;
Event_ eventExpression(key);
double measurement = 7; double measurement = 7;
TOAFactor factor(eventExpression, microphoneAt0, measurement, model); TOAFactor factor(key, sensorAt0, measurement, model);
} }
//***************************************************************************** //*****************************************************************************
TEST(TOAFactor, WholeEnchilada) { TEST(TOAFactor, WholeEnchilada) {
static const bool verbose = false; // Create sensors
// Create microphones
const double height = 0.5; const double height = 0.5;
vector<Point3> microphones; vector<Point3> sensors;
microphones.push_back(Point3(0, 0, height)); sensors.push_back(Point3(0, 0, height));
microphones.push_back(Point3(403 * cm, 0, height)); sensors.push_back(Point3(403 * cm, 0, height));
microphones.push_back(Point3(403 * cm, 403 * cm, height)); sensors.push_back(Point3(403 * cm, 403 * cm, height));
microphones.push_back(Point3(0, 403 * cm, 2 * height)); sensors.push_back(Point3(0, 403 * cm, 2 * height));
EXPECT_LONGS_EQUAL(4, microphones.size()); EXPECT_LONGS_EQUAL(4, sensors.size());
// microphones.push_back(Point3(200 * cm, 200 * cm, height)); // sensors.push_back(Point3(200 * cm, 200 * cm, height));
// Create a ground truth point // Create a ground truth point
const double timeOfEvent = 0; const double timeOfEvent = 0;
Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm); Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
// Simulate simulatedTOA // Simulate simulatedTOA
size_t K = microphones.size(); size_t K = sensors.size();
vector<double> simulatedTOA(K); vector<double> simulatedTOA(K);
TimeOfArrival toa; TimeOfArrival toa;
for (size_t i = 0; i < K; i++) { for (size_t i = 0; i < K; i++) {
simulatedTOA[i] = toa(groundTruthEvent, microphones[i]); simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
if (verbose) {
cout << "mic" << i << " = " << microphones[i] << endl;
cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
}
} }
// Now, estimate using non-linear optimization // Now, estimate using non-linear optimization
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
Key key = 12; Key key = 12;
Event_ eventExpression(key);
for (size_t i = 0; i < K; i++) { for (size_t i = 0; i < K; i++) {
graph.emplace_shared<TOAFactor>(eventExpression, microphones[i], graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], model);
simulatedTOA[i], model);
} }
/// Print the graph
if (verbose) GTSAM_PRINT(graph);
// Create initial estimate // Create initial estimate
Values initialEstimate; Values initialEstimate;
// Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
@ -104,16 +92,11 @@ TEST(TOAFactor, WholeEnchilada) {
Event estimatedEvent = groundTruthEvent.retract(delta); Event estimatedEvent = groundTruthEvent.retract(delta);
initialEstimate.insert(key, estimatedEvent); initialEstimate.insert(key, estimatedEvent);
// Print
if (verbose) initialEstimate.print("Initial Estimate:\n");
// Optimize using Levenberg-Marquardt optimization. // Optimize using Levenberg-Marquardt optimization.
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.setAbsoluteErrorTol(1e-10); params.setAbsoluteErrorTol(1e-10);
if (verbose) params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
if (verbose) result.print("Final Result:\n");
EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6)); EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
} }