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

Use Schur ordering

release/4.3a0
Frank Dellaert 2015-06-08 20:01:56 -04:00
parent f3e54ff916
commit 71caa40095
1 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
timing/timeSFMBAL.cpp
View File

@ -16,20 +16,20 @@
* @date June 6, 2015 * @date June 6, 2015
*/ */
#include <boost/foreach.hpp> #include <gtsam/slam/dataset.h>
#include <gtsam/base/timing.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/dataset.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <stddef.h> #include <stddef.h>
#include <stdexcept> #include <stdexcept>
#include <string> #include <string>
@ -39,18 +39,19 @@ using namespace gtsam;
//#define TERNARY //#define TERNARY
int main(int argc, char *argv[]) { int main(int argc, char* argv[]) {
typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor; typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor;
using symbol_shorthand::P; using symbol_shorthand::P;
// Load BAL file (default is tiny)
string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data db; SfM_data db;
bool success = readBAL(argc>1 ? argv[1] : defaultFilename, db); bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db);
if (!success) throw runtime_error("Could not access file!"); if (!success) throw runtime_error("Could not access file!");
// Build graph
SharedNoiseModel unit2 = noiseModel::Unit::Create(2); SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.number_tracks(); j++) {
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements)
graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
@ -58,9 +59,18 @@ int main(int argc, char *argv[]) {
Values initial = initialCamerasAndPointsEstimate(db); Values initial = initialCamerasAndPointsEstimate(db);
LevenbergMarquardtOptimizer lm(graph, initial); // Create Schur-complement ordering
vector<Key> pointKeys;
for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j));
Ordering schurOrdering = Ordering::colamdConstrainedFirst(graph, pointKeys, true);
// Optimize
LevenbergMarquardtParams params;
params.setOrdering(schurOrdering);
LevenbergMarquardtOptimizer lm(graph, initial, params);
Values actual = lm.optimize(); Values actual = lm.optimize();
tictoc_finishedIteration_();
tictoc_print_(); tictoc_print_();
return 0; return 0;