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