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

Optimization test succeeds, but only with 5th microphone...

release/4.3a0
dellaert 2014-12-10 19:00:52 +01:00
parent 2dcbc72d8d
commit 5d6e0bc753
1 changed files with 32 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -146,10 +146,10 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
// Create a noise model for the TOA error // Create a noise model for the TOA error
//static const double ms = 1e-3; static const double ms = 1e-3;
static const double cm = 1e-2; static const double cm = 1e-2;
typedef Eigen::Matrix<double, 1, 1> Vector1; typedef Eigen::Matrix<double, 1, 1> Vector1;
static SharedNoiseModel model(noiseModel::Unit::Create(1)); 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);
@ -222,51 +222,67 @@ TEST( TOAFactor, Construct ) {
//***************************************************************************** //*****************************************************************************
TEST( TOAFactor, WholeEnchilada ) { TEST( TOAFactor, WholeEnchilada ) {
static const bool verbose = false;
// Create microphones // Create microphones
const double height = 0.5;
vector<Point3> microphones; vector<Point3> microphones;
microphones.push_back(Point3(0, 0, 0)); microphones.push_back(Point3(0, 0, height));
microphones.push_back(Point3(403 * cm, 0, 0)); microphones.push_back(Point3(403 * cm, 0, height));
microphones.push_back(Point3(403 * cm, 403 * cm, 0)); microphones.push_back(Point3(403 * cm, 403 * cm, height));
microphones.push_back(Point3(0, 403 * cm, 0)); microphones.push_back(Point3(0, 403 * cm, height));
EXPECT_LONGS_EQUAL(4, microphones.size()); EXPECT_LONGS_EQUAL(4, microphones.size());
microphones.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, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
// Simulate measurements // Simulate measurements
vector<double> measurements(4); size_t K = microphones.size();
for (size_t i = 0; i < 4; i++) vector<double> measurements(K);
for (size_t i = 0; i < K; i++) {
measurements[i] = groundTruthEvent.toa(microphones[i]); measurements[i] = groundTruthEvent.toa(microphones[i]);
if (verbose) {
cout << "mic" << i << " = " << microphones[i] << endl;
cout << "z" << i << " = " << measurements[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;
Expression<Event> event_(key); Expression<Event> event_(key);
for (size_t i = 0; i < 4; i++) { for (size_t i = 0; i < K; i++) {
Expression<Point3> knownMicrophone_(microphones[i]); // constant expression Expression<Point3> knownMicrophone_(microphones[i]); // constant expression
graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model)); graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model));
} }
/// Print the graph /// Print the graph
// GTSAM_PRINT(graph); if (verbose)
GTSAM_PRINT(graph);
// Create initial estimate // Create initial estimate
Values initialEstimate; Values initialEstimate;
Event estimatedEvent(timeOfEvent + 10, 200 * cm, 150 * cm, 50 * 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); initialEstimate.insert(key, estimatedEvent);
// Print // Print
initialEstimate.print("Initial Estimate:\n"); if (verbose)
initialEstimate.print("Initial Estimate:\n");
// Optimize using Levenberg-Marquardt optimization. // Optimize using Levenberg-Marquardt optimization.
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.setVerbosity("ERROR");
params.setAbsoluteErrorTol(1e-10); params.setAbsoluteErrorTol(1e-10);
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); if (verbose)
params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
if (verbose)
result.print("Final Result:\n");
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));
} }