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

add in a natural ordering for testing. Test this code on other machines.

release/4.3a0
zhaoyang 2015-02-20 23:45:13 -05:00
parent 55729e0e69
commit 92f2e8e168
4 changed files with 80 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -18,6 +18,7 @@
// For an explanation of headers, see SFMExample.cpp
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
@ -40,7 +41,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 = findExampleDataFile(string(argv[1]));
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
@ -79,76 +80,102 @@ int main (int argc, char* argv[]) {
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
/** ---------------------------------------------------**/
double t_COLAMD_ordering, t_METIS_ordering; //, t_NATURAL_ordering;
LevenbergMarquardtParams params_using_COLAMD, params_using_METIS, params_using_NATURAL;
try {
double tic_t = clock();
params_using_METIS.setVerbosity("ERROR");
params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC;
tic_t = clock();
params_using_COLAMD.setVerbosity("ERROR");
params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC;
// tic_t = clock();
// params_using_NATURAL.setVerbosity("ERROR");
// params_using_NATURAL.ordering = Ordering::Create(Ordering::NATURAL, graph);
// t_NATURAL_ordering = (clock() - tic_t)/CLOCKS_PER_SEC;
} catch (exception& e) {
cout << e.what();
}
// expect they have different ordering results
if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
cout << "COLAMD and METIS produce the same ordering. "
<< "Problem here!!!" << endl;
}
/* with METIS, optimize the graph and print the results */
cout << "Optimize with METIS" << endl;
Values result_METIS;
double t_METIS_solving;
try {
double tic_t = clock();
LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_COLAMD);
result_METIS = lm_METIS.optimize();
t_METIS_solving = (clock() - tic_t)/CLOCKS_PER_SEC;
} catch (exception& e) {
cout << e.what();
}
/* With COLAMD, optimize the graph and print the results */
cout << "Optimize with COLAMD..." << endl;
Values result_COLAMD;
double t_COLAMD_ordering, t_COLAMD_solving;
double 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);
t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC;
tic_t = clock();
LevenbergMarquardtOptimizer lm(graph, initial, params_using_COLAMD);
result_COLAMD = lm.optimize();
LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
result_COLAMD = lm_COLAMD.optimize();
t_COLAMD_solving = (clock() - tic_t)/CLOCKS_PER_SEC;
} catch (exception& e) {
cout << e.what();
}
// disable optimizer with NATURAL since it doesn't converge on large problem
/* Use Natural ordering and solve both respectively */
// cout << "Solving with natural ordering: " << endl;
// Values result_NATURAL;
// double t_NATURAL_solving;
// try {
// double tic_t = clock();
// LevenbergMarquardtOptimizer lm_NATURAL(graph, initial, params_using_NATURAL);
// result_NATURAL = lm_NATURAL.optimize();
// t_NATURAL_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;
/** ---------------------------------------------------**/
/* with METIS, optimize the graph and print the results */
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);
t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC;
tic_t = clock();
LevenbergMarquardtOptimizer lm(graph, initial, params_using_METIS);
results_METIS = lm.optimize();
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") \
cout << boost::format("%1% point tracks and %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 << "Solving: " << t_COLAMD_solving << "seconds" << endl;
cout << "final error: " << graph.error(result_COLAMD) << endl;
cout << "METIS: " << endl;
cout << "Ordering: " << t_METIS_ordering << "seconds" << endl;
cout << "Solving: " << t_METIS_solving << "seconds" << endl;
cout << "Solving: " << t_METIS_solving << "seconds" << endl;
cout << "final error: " << graph.error(result_METIS) << endl;
// cout << "Natural: " << endl;
// cout << "Ordering: " << t_NATURAL_ordering << "seconds" << endl;
// cout << "Solving: " << t_NATURAL_solving << "seconds" << endl;
// cout << "final error: " << graph.error(result_NATURAL) << endl;
}
return 0;

6
gtsam/inference/Ordering.h
View File

@ -38,7 +38,7 @@ public:
/// Type of ordering to use
enum OrderingType {
COLAMD, METIS, CUSTOM
COLAMD, METIS, CUSTOM, NATURAL
};
typedef Ordering This; ///< Typedef to this class
@ -163,7 +163,7 @@ public:
/// Return a natural Ordering. Typically used by iterative solvers
template<class FACTOR>
static Ordering Natural(const FactorGraph<FACTOR> &fg) {
static Ordering natural(const FactorGraph<FACTOR> &fg) {
FastSet<Key> src = fg.keys();
std::vector<Key> keys(src.begin(), src.end());
std::stable_sort(keys.begin(), keys.end());
@ -196,6 +196,8 @@ public:
return colamd(graph);
case METIS:
return metis(graph);
case NATURAL:
return natural(graph);
case CUSTOM:
throw std::runtime_error(
"Ordering::Create error: called with CUSTOM ordering type.");

2
gtsam/linear/IterativeSolver.cpp
View File

@ -86,7 +86,7 @@ KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering)
/****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg)
: ordering_(Ordering::Natural(fg)) {
: ordering_(Ordering::natural(fg)) {
initialize(fg);
}

2
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -366,7 +366,7 @@ std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights,
Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
const SubgraphBuilderParameters &p = parameters_;
const Ordering inverse_ordering = Ordering::Natural(gfg);
const Ordering inverse_ordering = Ordering::natural(gfg);
const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;