diff --git a/CMakeLists.txt b/CMakeLists.txt index da8c4e81a..004ded5e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,6 @@ endif() # Set up options # Configurable Options -option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" OFF) # These do not currently work if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) endif() @@ -304,6 +303,9 @@ add_subdirectory(tests) # Build examples add_subdirectory(examples) +# Build timing +add_subdirectory(timing) + # Matlab toolbox if (GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) @@ -361,7 +363,7 @@ message(STATUS "================ Configuration Options ======================" message(STATUS "Build flags ") print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ") -print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") +print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'") if (DOXYGEN_FOUND) print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") endif() diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 19f487256..2e505c11e 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -38,21 +38,46 @@ endmacro() # # Add scripts that will serve as examples of how to use the library. A list of files or # glob patterns is specified, and one executable will be created for each matching .cpp -# file. These executables will not be installed. They are build with 'make all' if +# file. These executables will not be installed. They are built with 'make all' if # GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'. # # Usage example: # gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib") # # Arguments: -# globPatterns: The list of files or glob patterns from which to create unit tests, with -# one test created for each cpp file. e.g. "*.cpp", or +# globPatterns: The list of files or glob patterns from which to create examples, with +# one program created for each cpp file. e.g. "*.cpp", or # "A*.cpp;B*.cpp;MyExample.cpp". # excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass # an empty string "" if nothing needs to be excluded. # linkLibraries: The list of libraries to link to. macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries) - gtsamAddExamplesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}") + gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) +endmacro() + + +############################################################################### +# Macro: +# +# gtsamAddTimingGlob(globPatterns excludedFiles linkLibraries) +# +# Add scripts that time aspects of the library. A list of files or +# glob patterns is specified, and one executable will be created for each matching .cpp +# file. These executables will not be installed. They are not built with 'make all', +# but they may be built with 'make timing'. +# +# Usage example: +# gtsamAddTimingGlob("*.cpp" "DisabledTimingScript.cpp" "gtsam;GeographicLib") +# +# Arguments: +# globPatterns: The list of files or glob patterns from which to create programs, with +# one program created for each cpp file. e.g. "*.cpp", or +# "A*.cpp;B*.cpp;MyExample.cpp". +# excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass +# an empty string "" if nothing needs to be excluded. +# linkLibraries: The list of libraries to link to. +macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries) + gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS}) endmacro() @@ -63,6 +88,7 @@ enable_testing() option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON) +option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF) # Add option for combining unit tests if(MSVC OR XCODE_VERSION) @@ -80,6 +106,9 @@ endif() # Add examples target add_custom_target(examples) +# Add timing target +add_custom_target(timing) + # Include obsolete macros - will be removed in the near future include(GtsamTestingObsolete) @@ -180,7 +209,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) endmacro() -macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries) +macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName buildWithAll) # Get all script files file(GLOB script_files ${globPatterns}) @@ -220,20 +249,22 @@ macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries) target_link_libraries(${script_name} ${linkLibraries}) # Add target dependencies - add_dependencies(examples ${script_name}) + add_dependencies(${groupName} ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) endif() # Add TOPSRCDIR set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - - if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS) + + # Exclude from all or not - note weird variable assignment because we're in a macro + set(buildWithAll_on ${buildWithAll}) + if(NOT buildWithAll_on) # Exclude from 'make all' and 'make install' - set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON) + set_target_properties("${script_name}" PROPERTIES EXCLUDE_FROM_ALL ON) endif() # Configure target folder (for MSVC and Xcode) - set_property(TARGET ${script_name} PROPERTY FOLDER "Examples") + set_property(TARGET ${script_name} PROPERTY FOLDER "${groupName}") endforeach() endmacro() diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index 50145846e..66d3ec721 100644 --- a/gtsam/base/CMakeLists.txt +++ b/gtsam/base/CMakeLists.txt @@ -7,9 +7,3 @@ install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(base "gtsam" "gtsam" "${base_excluded_files}") -endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/discrete/CMakeLists.txt b/gtsam/discrete/CMakeLists.txt index 3c5602080..d78dff34f 100644 --- a/gtsam/discrete/CMakeLists.txt +++ b/gtsam/discrete/CMakeLists.txt @@ -6,9 +6,3 @@ install(FILES ${discrete_headers} DESTINATION include/gtsam/discrete) # Add all tests add_subdirectory(tests) - -# Build timing scripts -#if (GTSAM_BUILD_TIMING) -# gtsam_add_timing(discrete "${discrete_local_libs}") -#endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt index f72f965ea..dabdde45c 100644 --- a/gtsam/geometry/CMakeLists.txt +++ b/gtsam/geometry/CMakeLists.txt @@ -4,9 +4,3 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(geometry "gtsam" "gtsam" "${geometry_excluded_files}") -endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt index d5c37d976..c3df3a989 100644 --- a/gtsam/inference/CMakeLists.txt +++ b/gtsam/inference/CMakeLists.txt @@ -4,9 +4,3 @@ install(FILES ${inference_headers} DESTINATION include/gtsam/inference) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(inference "gtsam" "gtsam" "${inference_excluded_files}") -endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt index 29eb60b93..084c27057 100644 --- a/gtsam/linear/CMakeLists.txt +++ b/gtsam/linear/CMakeLists.txt @@ -4,8 +4,3 @@ install(FILES ${linear_headers} DESTINATION include/gtsam/linear) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(linear "gtsam" "gtsam" "${linear_excluded_files}") -endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/linear/tests/timeSLAMlike.cpp b/gtsam/linear/tests/timeSLAMlike.cpp deleted file mode 100644 index 479c03940..000000000 --- a/gtsam/linear/tests/timeSLAMlike.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 timeSLAMlike.cpp - * @brief Times solving of random SLAM-like graphs - * @author Richard Roberts - * @date Aug 30, 2010 - */ - -#include -#include - -#include -#include -#include -#include -#include - -using namespace gtsam; -using namespace std; -using namespace boost::lambda; - -typedef EliminationTree GaussianEliminationTree; - -static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); - -bool _pair_compare(const pair& a1, const pair& a2) { return a1.first < a2.first; } - -int main(int argc, char *argv[]) { - - size_t vardim = 3; - size_t blockdim = 3; - int nVars = 500; - size_t blocksPerVar = 5; - size_t varsPerBlock = 2; - size_t varSpread = 10; - - size_t nTrials = 10; - - double blockbuild, blocksolve; - - cout << "\n" << nVars << " variables of dimension " << vardim << ", " << - blocksPerVar << " blocks for each variable, blocks of dimension " << blockdim << " measure " << varsPerBlock << " variables\n"; - cout << nTrials << " trials\n"; - - boost::variate_generator > ri(boost::mt19937(), boost::uniform_int<>(-varSpread, varSpread)); - - ///////////////////////////////////////////////////////////////////////////// - // Timing test with blockwise Gaussian factor graphs - - { - // Build GFG's - cout << "Building SLAM-like Gaussian factor graphs... "; - cout.flush(); - boost::timer timer; - timer.restart(); - vector blockGfgs; - blockGfgs.reserve(nTrials); - for(size_t trial=0; trial > terms; terms.reserve(varsPerBlock); - if(c == 0 && d == 0) - // If it's the first factor, just make a prior - terms.push_back(make_pair(0, eye(vardim))); - else if(c != 0) { - // Generate a random Gaussian factor - for(size_t h=0; h nVars-1 || find_if(terms.begin(), terms.end(), - boost::bind(&pair::first, boost::lambda::_1) == Index(var)) != terms.end()); - Matrix A(blockdim, vardim); - for(size_t j=0; j #include #include -#include #include #include +#include using namespace std; using namespace gtsam; @@ -32,7 +32,7 @@ gtsam::Rot3 world_R_ECEF( 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); +gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ @@ -54,16 +54,16 @@ int main() { gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3)); + Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); @@ -72,7 +72,7 @@ int main() { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); @@ -99,7 +99,7 @@ int main() { GaussianFactorGraph graph; gttic_(LinearizeTiming); for(size_t i = 0; i < 100000; ++i) { - GaussianFactor::shared_ptr g = f.linearize(values, ordering); + GaussianFactor::shared_ptr g = f.linearize(values); graph.push_back(g); } gttoc_(LinearizeTiming); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index cf81dc762..008dbb78d 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -14,16 +14,3 @@ if(MSVC) set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp" APPEND PROPERTY COMPILE_FLAGS "/bigobj") endif() - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - # Subdirectory target for timing - does not actually execute the scripts - add_custom_target(timing.tests) - set(is_test FALSE) - - # Build grouped benchmarks - gtsam_add_grouped_scripts("tests" # Use subdirectory as group label - "time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts - "${tests_full_libs}" "${tests_full_libs}" "${tests_exclude}" # Pass in linking and exclusion lists - ${is_test}) -endif (GTSAM_BUILD_TIMING) diff --git a/timing/CMakeLists.txt b/timing/CMakeLists.txt new file mode 100644 index 000000000..fc16d3ac8 --- /dev/null +++ b/timing/CMakeLists.txt @@ -0,0 +1,3 @@ +gtsamAddTimingGlob("*.cpp" "" "gtsam") + +target_link_libraries(timeGaussianFactorGraph CppUnitLite) diff --git a/tests/timeBatch.cpp b/timing/timeBatch.cpp similarity index 100% rename from tests/timeBatch.cpp rename to timing/timeBatch.cpp diff --git a/gtsam/geometry/tests/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp similarity index 96% rename from gtsam/geometry/tests/timeCalibratedCamera.cpp rename to timing/timeCalibratedCamera.cpp index 1833579d9..76813c455 100644 --- a/gtsam/geometry/tests/timeCalibratedCamera.cpp +++ b/timing/timeCalibratedCamera.cpp @@ -27,11 +27,11 @@ int main() { int n = 100000; - const Pose3 pose1((Matrix)(Mat(3,3) << + const Pose3 pose1(Matrix3((Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + )), Point3(0,0,0.5)); const CalibratedCamera camera(pose1); diff --git a/gtsam/linear/tests/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp similarity index 78% rename from gtsam/linear/tests/timeFactorOverhead.cpp rename to timing/timeFactorOverhead.cpp index 6e5d6b9ad..97c40e6f4 100644 --- a/gtsam/linear/tests/timeFactorOverhead.cpp +++ b/timing/timeFactorOverhead.cpp @@ -16,30 +16,29 @@ * @date Aug 20, 2010 */ +#include +#include #include #include -#include +#include #include -#include #include using namespace gtsam; using namespace std; -typedef EliminationTree GaussianEliminationTree; - static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); int main(int argc, char *argv[]) { - Index key = 0; + Key key = 0; size_t vardim = 2; size_t blockdim = 1; - size_t nBlocks = 2000; + size_t nBlocks = 4000; - size_t nTrials = 10; + size_t nTrials = 20; double blockbuild, blocksolve, combbuild, combsolve; @@ -54,8 +53,7 @@ int main(int argc, char *argv[]) { // Build GFG's cout << "Building blockwise Gaussian factor graphs... "; cout.flush(); - boost::timer timer; - timer.restart(); + gttic_(blockbuild); vector blockGfgs; blockGfgs.reserve(nTrials); for(size_t trial=0; trial(key, A, b, noise)); } } - blockbuild = timer.elapsed(); + gttoc_(blockbuild); + tictoc_getNode(blockbuildNode, blockbuild); + blockbuild = blockbuildNode->secs(); cout << blockbuild << " s" << endl; // Solve GFG's cout << "Solving blockwise Gaussian factor graphs... "; cout.flush(); - timer.restart(); + gttic_(blocksolve); for(size_t trial=0; trialeliminate(&EliminateQR)); - VectorValues soln(optimize(*gbn)); + GaussianBayesNet::shared_ptr gbn = blockGfgs[trial].eliminateSequential(); + VectorValues soln = gbn->optimize(); } - blocksolve = timer.elapsed(); + gttoc_(blocksolve); + tictoc_getNode(blocksolveNode, blocksolve); + blocksolve = blocksolveNode->secs(); cout << blocksolve << " s" << endl; } @@ -97,8 +99,7 @@ int main(int argc, char *argv[]) { // Build GFG's cout << "Building combined-factor Gaussian factor graphs... "; cout.flush(); - boost::timer timer; - timer.restart(); + gttic_(combbuild); vector combGfgs; for(size_t trial=0; trial(key, Acomb, bcomb, + noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0))); } - combbuild = timer.elapsed(); + gttoc(combbuild); + tictoc_getNode(combbuildNode, combbuild); + combbuild = combbuildNode->secs(); cout << combbuild << " s" << endl; // Solve GFG's cout << "Solving combined-factor Gaussian factor graphs... "; cout.flush(); - timer.restart(); + gttic_(combsolve); for(size_t trial=0; trialeliminate(&EliminateQR)); - VectorValues soln(optimize(*gbn)); + GaussianBayesNet::shared_ptr gbn = combGfgs[trial].eliminateSequential(); + VectorValues soln = gbn->optimize(); } - combsolve = timer.elapsed(); + gttoc_(combsolve); + tictoc_getNode(combsolveNode, combsolve); + combsolve = combsolveNode->secs(); cout << combsolve << " s" << endl; } diff --git a/gtsam/linear/tests/timeGaussianFactor.cpp b/timing/timeGaussianFactor.cpp similarity index 92% rename from gtsam/linear/tests/timeGaussianFactor.cpp rename to timing/timeGaussianFactor.cpp index 0ffdd1cfa..261fbac48 100644 --- a/gtsam/linear/tests/timeGaussianFactor.cpp +++ b/timing/timeGaussianFactor.cpp @@ -22,7 +22,7 @@ using namespace std; #include -#include // for operator += in Ordering +#include #include #include @@ -33,7 +33,7 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -static const Index _x1_=1, _x2_=2, _l1_=3; +static const Key _x1_=1, _x2_=2, _l1_=3; /* * Alex's Machine @@ -53,7 +53,7 @@ static const Index _x1_=1, _x2_=2, _l1_=3; int main() { // create a linear factor - Matrix Ax2 = (Mat(8, 2) << + Matrix Ax2 = (Matrix(8, 2) << // x2 -5., 0., +0.,-5., @@ -65,7 +65,7 @@ int main() +0.,10. ); - Matrix Al1 = (Mat(8, 10) << + Matrix Al1 = (Matrix(8, 10) << // l1 5., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., @@ -77,7 +77,7 @@ int main() 0., 0.,1.,2.,3.,4.,5.,6.,7.,8. ); - Matrix Ax1 = (Mat(8, 2) << + Matrix Ax1 = (Matrix(8, 2) << // x1 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., @@ -108,7 +108,8 @@ int main() JacobianFactor::shared_ptr factor; for(int i = 0; i < n; i++) - conditional = JacobianFactor(combined).eliminateFirst(); + boost::tie(conditional, factor) = + JacobianFactor(combined).eliminate(Ordering(boost::assign::list_of(_x2_))); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; diff --git a/tests/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp similarity index 91% rename from tests/timeGaussianFactorGraph.cpp rename to timing/timeGaussianFactorGraph.cpp index 00d24bd13..a3157729e 100644 --- a/tests/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -29,10 +29,10 @@ using namespace boost::assign; /* ************************************************************************* */ // Create a Kalman smoother for t=1:T and optimize double timeKalmanSmoother(int T) { - pair smoother_ordering = createSmoother(T); - GaussianFactorGraph& smoother(smoother_ordering.first); + GaussianFactorGraph smoother = createSmoother(T); clock_t start = clock(); - GaussianSequentialSolver(smoother).optimize(); + // Keys will come out sorted since keys() returns a set + smoother.optimize(Ordering(smoother.keys())); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; @@ -40,12 +40,10 @@ double timeKalmanSmoother(int T) { /* ************************************************************************* */ // Create a planar factor graph and optimize -// todo: use COLAMD ordering again (removed when linear baked-in ordering added) double timePlanarSmoother(int N, bool old = true) { - boost::tuple pg = planarGraph(N); - GaussianFactorGraph& fg(pg.get<0>()); + GaussianFactorGraph fg = planarGraph(N).get<0>(); clock_t start = clock(); - GaussianSequentialSolver(fg).optimize(); + fg.optimize(); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; @@ -53,12 +51,10 @@ double timePlanarSmoother(int N, bool old = true) { /* ************************************************************************* */ // Create a planar factor graph and eliminate -// todo: use COLAMD ordering again (removed when linear baked-in ordering added) double timePlanarSmootherEliminate(int N, bool old = true) { - boost::tuple pg = planarGraph(N); - GaussianFactorGraph& fg(pg.get<0>()); + GaussianFactorGraph fg = planarGraph(N).get<0>(); clock_t start = clock(); - GaussianSequentialSolver(fg).eliminate(); + fg.eliminateMultifrontal(); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; diff --git a/tests/timeIncremental.cpp b/timing/timeIncremental.cpp similarity index 100% rename from tests/timeIncremental.cpp rename to timing/timeIncremental.cpp diff --git a/gtsam/slam/tests/timeLago.cpp b/timing/timeLago.cpp similarity index 100% rename from gtsam/slam/tests/timeLago.cpp rename to timing/timeLago.cpp diff --git a/gtsam/base/tests/timeMatrix.cpp b/timing/timeMatrix.cpp similarity index 99% rename from gtsam/base/tests/timeMatrix.cpp rename to timing/timeMatrix.cpp index 2cd0a8b19..7e1c1b5be 100644 --- a/gtsam/base/tests/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -189,7 +189,7 @@ double timeColumn(size_t reps) { */ double timeHouseholder(size_t reps) { // create a matrix - Matrix Abase = Mat(4, 7) << + Matrix Abase = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00, -5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10, 0, 2, diff --git a/gtsam/base/tests/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp similarity index 100% rename from gtsam/base/tests/timeMatrixOps.cpp rename to timing/timeMatrixOps.cpp diff --git a/gtsam/geometry/tests/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp similarity index 98% rename from gtsam/geometry/tests/timePinholeCamera.cpp rename to timing/timePinholeCamera.cpp index de1fa1279..66a8ca099 100644 --- a/gtsam/geometry/tests/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -28,7 +28,7 @@ int main() { int n = 1000000; - const Pose3 pose1((Matrix)(Mat(3,3) << + const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/geometry/tests/timePose2.cpp b/timing/timePose2.cpp similarity index 99% rename from gtsam/geometry/tests/timePose2.cpp rename to timing/timePose2.cpp index 28044068c..94bd78ef9 100644 --- a/gtsam/geometry/tests/timePose2.cpp +++ b/timing/timePose2.cpp @@ -58,7 +58,7 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; - *H1 = (Mat(3,3) << + *H1 = (Matrix(3,3) << -c, -s, dt1, s, -c, dt2, 0.0, 0.0,-1.0); diff --git a/gtsam/geometry/tests/timePose3.cpp b/timing/timePose3.cpp similarity index 90% rename from gtsam/geometry/tests/timePose3.cpp rename to timing/timePose3.cpp index 13e630706..fa8ef7b6c 100644 --- a/gtsam/geometry/tests/timePose3.cpp +++ b/timing/timePose3.cpp @@ -37,8 +37,8 @@ int main() double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = (Vec(6) << x, y, z, 0.1, 0.2, -0.1); - Pose3 T = Pose3::Expmap((Vec(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v); + Vector v = (Vector(6) << x, y, z, 0.1, 0.2, -0.1); + Pose3 T = Pose3::Expmap((Vector(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v); Matrix H1,H2; TEST(retract, T.retract(v)) diff --git a/gtsam/geometry/tests/timeRot2.cpp b/timing/timeRot2.cpp similarity index 100% rename from gtsam/geometry/tests/timeRot2.cpp rename to timing/timeRot2.cpp diff --git a/gtsam/geometry/tests/timeRot3.cpp b/timing/timeRot3.cpp similarity index 100% rename from gtsam/geometry/tests/timeRot3.cpp rename to timing/timeRot3.cpp diff --git a/gtsam/geometry/tests/timeStereoCamera.cpp b/timing/timeStereoCamera.cpp similarity index 96% rename from gtsam/geometry/tests/timeStereoCamera.cpp rename to timing/timeStereoCamera.cpp index bfe4b5aaa..dce622720 100644 --- a/gtsam/geometry/tests/timeStereoCamera.cpp +++ b/timing/timeStereoCamera.cpp @@ -27,7 +27,7 @@ int main() { int n = 100000; - const Pose3 pose1((Matrix)(Mat(3,3) << + const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/base/tests/timeTest.cpp b/timing/timeTest.cpp similarity index 100% rename from gtsam/base/tests/timeTest.cpp rename to timing/timeTest.cpp diff --git a/gtsam/base/tests/timeVirtual.cpp b/timing/timeVirtual.cpp similarity index 100% rename from gtsam/base/tests/timeVirtual.cpp rename to timing/timeVirtual.cpp diff --git a/gtsam/base/tests/timeVirtual2.cpp b/timing/timeVirtual2.cpp similarity index 100% rename from gtsam/base/tests/timeVirtual2.cpp rename to timing/timeVirtual2.cpp diff --git a/tests/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp similarity index 100% rename from tests/timeiSAM2Chain.cpp rename to timing/timeiSAM2Chain.cpp