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

30
timing/timeSFMBAL.cpp
View File

@ -16,20 +16,20 @@
* @date June 6, 2015
*/
#include <boost/foreach.hpp>
#include <gtsam/base/timing.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.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/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <stddef.h>
#include <stdexcept>
#include <string>
@ -43,14 +43,15 @@ int main(int argc, char *argv[]) {
typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor;
using symbol_shorthand::P;
// Load BAL file (default is tiny)
string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data db;
bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db);
if (!success) throw runtime_error("Could not access file!");
// Build graph
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements)
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);
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();
tictoc_finishedIteration_();
tictoc_print_();
return 0;