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

fix bugs in timing, duplicate graph

release/4.3a0
zhaoyang 2015-02-19 16:00:21 -05:00
parent c2a223ddbb
commit 55729e0e69
1 changed files with 29 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -40,7 +40,7 @@ int main (int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc>1) filename = string(argv[1]);
if (argc>1) filename = findExampleDataFile(string(argv[1]));
// Load the SfM data from file
SfM_data mydata;
@ -48,7 +48,7 @@ int main (int argc, char* argv[]) {
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
NonlinearFactorGraph graph_for_COLAMD;
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
@ -60,15 +60,15 @@ int main (int argc, char* argv[]) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph_for_COLAMD.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph_for_COLAMD.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph_for_COLAMD.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
// Create initial estimate
Values initial;
@ -76,9 +76,6 @@ int main (int argc, char* argv[]) {
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
NonlinearFactorGraph graph_for_METIS = graph_for_COLAMD.clone();
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
@ -86,31 +83,28 @@ int main (int argc, char* argv[]) {
cout << "Optimize with COLAMD..." << endl;
Values result_COLAMD;
double t_COLAMD_ordering, t_COLAMD_solving;
try {
double tic_t = clock();
LevenbergMarquardtParams params_using_COLAMD;
params_using_COLAMD.setVerbosity("ERROR");
params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph_for_COLAMD);
params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
double toc_t = (clock() - tic_t)/CLOCKS_PER_SEC;
t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC;
tic_t = clock();
LevenbergMarquardtOptimizer lm(graph_for_COLAMD, initial, params_using_COLAMD);
LevenbergMarquardtOptimizer lm(graph, initial, params_using_COLAMD);
result_COLAMD = lm.optimize();
tic_t = clock();
cout << "Ordering: " << toc_t << "seconds" << endl;
cout << "Solving: " << (clock() - tic_t)/CLOCKS_PER_SEC << "seconds" << endl;
t_COLAMD_solving = (clock() - tic_t)/CLOCKS_PER_SEC;
} catch (exception& e) {
cout << e.what();
}
cout << endl << endl;
// To see the error, check SFMExample_bal.cpp file
//cout << "final error: " << graph.error(result_COLAMD) << endl;
@ -120,30 +114,42 @@ int main (int argc, char* argv[]) {
cout << "Optimize with METIS" << endl;
Values results_METIS;
double t_METIS_ordering, t_METIS_solving;
try {
double tic_t = clock();
LevenbergMarquardtParams params_using_METIS;
params_using_METIS.setVerbosity("ERROR");
params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph_for_METIS);
params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
double toc_t = (clock() - tic_t)/CLOCKS_PER_SEC;
t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC;
tic_t = clock();
LevenbergMarquardtOptimizer lm(graph_for_METIS, initial, params_using_METIS);
LevenbergMarquardtOptimizer lm(graph, initial, params_using_METIS);
results_METIS = lm.optimize();
tic_t = clock();
cout << "Ordering: " << toc_t << "seconds" << endl;
cout << "Solving: " << (clock() - tic_t)/CLOCKS_PER_SEC << "seconds" << endl;
t_METIS_solving = (clock() - tic_t)/CLOCKS_PER_SEC;
} catch (exception& e) {
cout << e.what();
}
{
// printing the result
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("read %1% tracks on %2% cameras\n") \
% mydata.number_tracks() % mydata.number_cameras() \
<< endl;
cout << "COLAMD: " << endl;
cout << "Ordering: " << t_COLAMD_ordering << "seconds" << endl;
cout << "Solving: " << t_COLAMD_solving << "seconds" << endl;
cout << "METIS: " << endl;
cout << "Ordering: " << t_METIS_ordering << "seconds" << endl;
cout << "Solving: " << t_METIS_solving << "seconds" << endl;
}
return 0;
}