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:
/**
* Most genral constructor with two expressions
* Most general 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
* @param speed optional speed of signal, in m/sec
*/
TOAFactor(const Expression<Event>& eventExpression,
const Expression<Point3>& sensorExpression, double toaMeasurement,
const SharedNoiseModel& model,
const TimeOfArrival& toa = TimeOfArrival())
const SharedNoiseModel& model, double speed = 330)
: ExpressionFactor<double>(
model, toaMeasurement,
Double_(toa, eventExpression, sensorExpression)) {}
Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}
/**
* Constructor with fixed sensor
@ -55,9 +54,9 @@ class TOAFactor : public ExpressionFactor<double> {
*/
TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
double toaMeasurement, const SharedNoiseModel& model,
const TimeOfArrival& toa = TimeOfArrival())
double speed = 330)
: TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
model, toa) {}
model, speed) {}
};
} // 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 Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0, 0, 0);
static const Point3 sensorAt0(0, 0, 0);
//*****************************************************************************
TEST(TOAFactor, NewWay) {
Key key = 12;
Event_ eventExpression(key);
double measurement = 7;
TOAFactor factor(eventExpression, microphoneAt0, measurement, model);
TOAFactor factor(key, sensorAt0, measurement, model);
}
//*****************************************************************************
TEST(TOAFactor, WholeEnchilada) {
static const bool verbose = false;
// Create microphones
// Create sensors
const double height = 0.5;
vector<Point3> microphones;
microphones.push_back(Point3(0, 0, height));
microphones.push_back(Point3(403 * cm, 0, height));
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));
vector<Point3> sensors;
sensors.push_back(Point3(0, 0, height));
sensors.push_back(Point3(403 * cm, 0, height));
sensors.push_back(Point3(403 * cm, 403 * cm, height));
sensors.push_back(Point3(0, 403 * cm, 2 * height));
EXPECT_LONGS_EQUAL(4, sensors.size());
// sensors.push_back(Point3(200 * cm, 200 * cm, height));
// Create a ground truth point
const double timeOfEvent = 0;
Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
// Simulate simulatedTOA
size_t K = microphones.size();
size_t K = sensors.size();
vector<double> simulatedTOA(K);
TimeOfArrival toa;
for (size_t i = 0; i < K; i++) {
simulatedTOA[i] = toa(groundTruthEvent, microphones[i]);
if (verbose) {
cout << "mic" << i << " = " << microphones[i] << endl;
cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
}
simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
}
// Now, estimate using non-linear optimization
NonlinearFactorGraph graph;
Key key = 12;
Event_ eventExpression(key);
for (size_t i = 0; i < K; i++) {
graph.emplace_shared<TOAFactor>(eventExpression, microphones[i],
simulatedTOA[i], model);
graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], model);
}
/// Print the graph
if (verbose) GTSAM_PRINT(graph);
// Create initial estimate
Values initialEstimate;
// Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
@ -104,16 +92,11 @@ TEST(TOAFactor, WholeEnchilada) {
Event estimatedEvent = groundTruthEvent.retract(delta);
initialEstimate.insert(key, estimatedEvent);
// Print
if (verbose) initialEstimate.print("Initial Estimate:\n");
// Optimize using Levenberg-Marquardt optimization.
LevenbergMarquardtParams params;
params.setAbsoluteErrorTol(1e-10);
if (verbose) params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
if (verbose) result.print("Final Result:\n");
EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
}