Added smart timing
parent
6547f1dbc7
commit
4c8ba55d0e
|
@ -67,7 +67,7 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph,
|
|||
// Set parameters to be similar to ceres
|
||||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams::SetCeresDefaults(¶ms);
|
||||
params.setVerbosityLM("SUMMARY");
|
||||
// params.setVerbosityLM("SUMMARY");
|
||||
|
||||
if (gUseSchur) {
|
||||
// Create Schur-complement ordering
|
||||
|
@ -81,8 +81,11 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph,
|
|||
}
|
||||
|
||||
// Optimize
|
||||
LevenbergMarquardtOptimizer lm(graph, initial, params);
|
||||
Values result = lm.optimize();
|
||||
{
|
||||
gttic_(optimize);
|
||||
LevenbergMarquardtOptimizer lm(graph, initial, params);
|
||||
Values result = lm.optimize();
|
||||
}
|
||||
|
||||
tictoc_finishedIteration_();
|
||||
tictoc_print_();
|
||||
|
|
|
@ -35,12 +35,12 @@ int main(int argc, char* argv[]) {
|
|||
// Build graph using conventional GeneralSFMFactor
|
||||
NonlinearFactorGraph graph;
|
||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
||||
Point3_ nav_point_(P(j));
|
||||
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
|
||||
size_t i = m.first;
|
||||
Point2 z = m.second;
|
||||
Pose3_ navTcam_(C(i));
|
||||
Cal3Bundler_ calibration_(K(i));
|
||||
Point3_ nav_point_(P(j));
|
||||
graph.addExpressionFactor(
|
||||
gNoiseModel, z,
|
||||
uncalibrate(calibration_,
|
||||
|
|
|
@ -0,0 +1,55 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file timeSFMBALsmart.cpp
|
||||
* @brief time SFM with BAL file, SmartProjectionFactor
|
||||
* @author Frank Dellaert
|
||||
* @date Feb 26, 2016
|
||||
*/
|
||||
|
||||
#include "timeSFMBAL.h"
|
||||
|
||||
#include <gtsam/slam/SmartProjectionFactor.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef PinholeCamera<Cal3Bundler> Camera;
|
||||
typedef SmartProjectionFactor<Camera> SfmFactor;
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
// parse options and read BAL file
|
||||
SfM_data db = preamble(argc, argv);
|
||||
|
||||
// Add smart factors to graph
|
||||
NonlinearFactorGraph graph;
|
||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
||||
auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
|
||||
for (const SfM_Measurement& m : db.tracks[j].measurements) {
|
||||
size_t i = m.first;
|
||||
Point2 z = m.second;
|
||||
smartFactor->add(z, C(i));
|
||||
}
|
||||
graph.push_back(smartFactor);
|
||||
}
|
||||
|
||||
Values initial;
|
||||
size_t i = 0;
|
||||
gUseSchur = false;
|
||||
for (const SfM_Camera& camera : db.cameras)
|
||||
initial.insert(C(i++), camera);
|
||||
|
||||
return optimize(db, graph, initial);
|
||||
}
|
Loading…
Reference in New Issue