/* ---------------------------------------------------------------------------- * 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 timeSequentialOnDataset.cpp * @brief * @author Richard Roberts * @created Oct 7, 2010 */ #include #include #include #include using namespace std; using namespace gtsam; using namespace boost; int main(int argc, char *argv[]) { string datasetname; if(argc > 1) datasetname = argv[1]; else datasetname = "intel"; pair, shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose data.first->addPrior(0, Pose2(), sharedSigma(Pose2::Dim(), 0.0005)); tic_("Z 1 order"); Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second)); toc_("Z 1 order"); tictoc_print_(); tic_("Z 2 linearize"); GaussianFactorGraph::shared_ptr gfg(data.first->linearize(*data.second, *ordering)); toc_("Z 2 linearize"); tictoc_print_(); for(size_t trial = 0; trial < 10; ++trial) { tic_("Z 3 solve"); VectorValues soln(*GaussianSequentialSolver(*gfg).optimize()); toc_("Z 3 solve"); tictoc_print_(); } return 0; }