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

Added in real experimental data gathered by Jay at KU Leuven

release/4.3a0
dellaert 2014-12-10 21:12:38 +01:00
parent d17caa5487
commit dc84b6589e
1 changed files with 80 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,11 +36,11 @@ public:
* @param toaMeasurement time of arrival at microphone
* @param model noise model
*/
TOAFactor(const Expression<Event>& event_,
TOAFactor(const Expression<Event>& eventExpression,
const Expression<Point3>& microphone_, double toaMeasurement,
const SharedNoiseModel& model) :
ExpressionFactor<double>(model, toaMeasurement,
double_(&Event::toa, event_, microphone_)) {
double_(&Event::toa, eventExpression, microphone_)) {
}
};
@ -60,7 +60,7 @@ 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);
@ -69,10 +69,10 @@ static const Point3 microphoneAt0;
//*****************************************************************************
TEST( TOAFactor, Construct ) {
Key key = 12;
Expression<Event> event_(key);
Expression<Event> eventExpression(key);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
double measurement = 7;
TOAFactor factor(event_, knownMicrophone_, measurement, model);
TOAFactor factor(eventExpression, knownMicrophone_, measurement, model);
}
//*****************************************************************************
@ -88,7 +88,7 @@ TEST( TOAFactor, WholeEnchilada ) {
microphones.push_back(Point3(403 * cm, 403 * cm, height));
microphones.push_back(Point3(0, 403 * cm, 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;
@ -101,17 +101,18 @@ TEST( TOAFactor, WholeEnchilada ) {
measurements[i] = groundTruthEvent.toa(microphones[i]);
if (verbose) {
cout << "mic" << i << " = " << microphones[i] << endl;
cout << "z" << i << " = " << measurements[i]/ms << endl;
cout << "z" << i << " = " << measurements[i] / ms << endl;
}
}
// Now, estimate using non-linear optimization
NonlinearFactorGraph graph;
Key key = 12;
Expression<Event> event_(key);
Expression<Event> eventExpression(key);
for (size_t i = 0; i < K; i++) {
Expression<Point3> knownMicrophone_(microphones[i]); // constant expression
graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model));
graph.add(
TOAFactor(eventExpression, knownMicrophone_, measurements[i], model));
}
/// Print the graph
@ -121,7 +122,8 @@ TEST( TOAFactor, WholeEnchilada ) {
// Create initial estimate
Values initialEstimate;
//Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
Vector4 delta; delta << 0.1, 0.1, -0.1, 0.1;
Vector4 delta;
delta << 0.1, 0.1, -0.1, 0.1;
Event estimatedEvent = groundTruthEvent.retract(delta);
initialEstimate.insert(key, estimatedEvent);
@ -141,6 +143,74 @@ TEST( TOAFactor, WholeEnchilada ) {
EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
}
//*****************************************************************************
/// Test real data
TEST( TOAFactor, RealExperiment1 ) {
static const bool verbose = true;
// Create microphones
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, height));
EXPECT_LONGS_EQUAL(4, microphones.size());
vector<Vector4> data(15);
size_t i = 0;
data[i++] << 1.2648, 1.2648, 1.2677, 1.2643;
data[i++] << 1.7329, 1.7347, 1.7354, 1.7338;
data[i++] << 2.2475, 2.2551, 2.2538, 2.2474;
data[i++] << 2.6945, 2.696, 2.6958, 2.694;
data[i++] << 3.1486, 3.152, 3.1513, 3.1501;
data[i++] << 3.6145, 3.611, 3.6076, 3.6067;
data[i++] << 4.1003, 4.1004, 4.099, 4.0972;
data[i++] << 4.5732, 4.568, 4.5667, 4.5722;
data[i++] << 5.0482, 5.0458, 5.0443, 5.0453;
data[i++] << 5.5311, 5.5256, 5.5254, 5.5305;
data[i++] << 5.9908, 5.9856, 5.9853, 5.9905;
data[i++] << 6.4575, 6.4524, 6.4527, 6.4579;
data[i++] << 6.8983, 6.8971, 6.8984, 6.9016;
data[i++] << 7.3581, 7.3524, 7.3538, 7.3588;
data[i++] << 7.8286, 7.8286, 7.8302, 7.8353;
// Create unknowns and initial estimate
Event nullEvent;
Values initialEstimate;
vector<Expression<Event> > eventExpressions;
for (size_t j = 0; j < 15; j++) {
initialEstimate.insert(j, nullEvent);
eventExpressions.push_back(Expression<Event>(j));
}
// Print
if (verbose)
initialEstimate.print("Initial Estimate:\n");
// Create factor graph and initial estimate
NonlinearFactorGraph graph;
for (size_t i = 0; i < 4; i++) {
Expression<Point3> mic_(microphones[i]); // constant expression
for (size_t j = 0; j < 15; j++)
graph.add(TOAFactor(eventExpressions[j], mic_, data[j][i], model));
}
/// Print the graph
if (verbose)
GTSAM_PRINT(graph);
// 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");
}
//*****************************************************************************
int main() {