timeSFMBAL.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/sfm/SfmData.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
 
#include <string>
#include <vector>
 
using namespace std;
using namespace gtsam;
using symbol_shorthand::C;
using symbol_shorthand::K;
using symbol_shorthand::P;
 
static bool gUseSchur = true;
static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
 
// parse options and read BAL file
SfmData preamble(int argc, char* argv[]) {
  // primitive argument parsing:
  if (argc > 2) {
    if (strcmp(argv[1], "--colamd"))
      gUseSchur = false;
    else
      throw runtime_error("Usage: timeSFMBALxxx [--colamd] [BALfile]");
  }
 
  // Load BAL file
  SfmData db;
  string filename;
  if (argc > 1)
    filename = argv[argc - 1];
  else
    filename = findExampleDataFile("dubrovnik-16-22106-pre");
  return SfmData::FromBalFile(filename);
}
 
// Create ordering and optimize
int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
             const Values& initial, bool separateCalibration = false) {
  using symbol_shorthand::P;
 
  // Set parameters to be similar to ceres
  LevenbergMarquardtParams params;
  LevenbergMarquardtParams::SetCeresDefaults(&params);
//  params.setLinearSolverType("SEQUENTIAL_CHOLESKY");
//  params.setVerbosityLM("SUMMARY");
 
  if (gUseSchur) {
    // Create Schur-complement ordering
    Ordering ordering;
    for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j));
    for (size_t i = 0; i < db.numberCameras(); i++) {
      ordering.push_back(C(i));
      if (separateCalibration) ordering.push_back(K(i));
    }
    params.setOrdering(ordering);
  }
 
  // Optimize
  {
    gttic_(optimize);
    LevenbergMarquardtOptimizer lm(graph, initial, params);
    Values result = lm.optimize();
  }
 
  tictoc_finishedIteration_();
  tictoc_print_();
 
  return 0;
}