From 17c7e0200354611ea4ff682eadec65965f7dead4 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 7 Jun 2014 19:00:16 -0700 Subject: [PATCH 01/15] Added support for timing scripts to build tools --- CMakeLists.txt | 1 - cmake/GtsamTesting.cmake | 47 +++++++++++++++++++++++++++++++++------- 2 files changed, 39 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 30699dd01..010d4de65 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() diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 19f487256..6195dc510 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" "FALSE") 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 "Enable/Disable building of timing scripts" OFF) # These do not currently work # Add option for combining unit tests if(MSVC OR XCODE_VERSION) @@ -80,6 +106,11 @@ endif() # Add examples target add_custom_target(examples) +# Add timing target +if(GTSAM_BUILD_TIMING) + add_custom_target(timing) +endif() + # Include obsolete macros - will be removed in the near future include(GtsamTestingObsolete) @@ -180,7 +211,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,7 +251,7 @@ 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() @@ -228,12 +259,12 @@ macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries) # Add TOPSRCDIR set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS) + if(NOT buildWithAll) # Exclude from 'make all' and 'make install' set_target_properties(${target_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() From 2591f7dca2fb7d532cab72d2da17a11998015d82 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 7 Jun 2014 19:01:13 -0700 Subject: [PATCH 02/15] Fixed missing gtsam_unstable/partition tests --- gtsam_unstable/partition/CMakeLists.txt | 4 +--- gtsam_unstable/partition/tests/CMakeLists.txt | 2 ++ 2 files changed, 3 insertions(+), 3 deletions(-) create mode 100644 gtsam_unstable/partition/tests/CMakeLists.txt diff --git a/gtsam_unstable/partition/CMakeLists.txt b/gtsam_unstable/partition/CMakeLists.txt index d8d254513..fa5c336c3 100644 --- a/gtsam_unstable/partition/CMakeLists.txt +++ b/gtsam_unstable/partition/CMakeLists.txt @@ -2,6 +2,4 @@ file(GLOB partition_headers "*.h") install(FILES ${partition_headers} DESTINATION include/gtsam_unstable/parition) -set(ignore_test "tests/testNestedDissection.cpp") -# Add all tests -gtsamAddTestsGlob(partition_unstable "tests/*.cpp" "${ignore_test}" "gtsam_unstable;metis") +add_subdirectory(tests) diff --git a/gtsam_unstable/partition/tests/CMakeLists.txt b/gtsam_unstable/partition/tests/CMakeLists.txt new file mode 100644 index 000000000..5a0f2d38c --- /dev/null +++ b/gtsam_unstable/partition/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +set(ignore_test "testNestedDissection.cpp") +gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable") From fe235b120916ce624666205e9fac5c5c21bacfeb Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 7 Jun 2014 19:02:11 -0700 Subject: [PATCH 03/15] Building timing scripts using new timing script support in GtsamTesting.cmake. Fixed compile errors in timing scripts but disabled a couple. --- CMakeLists.txt | 3 +++ gtsam/base/CMakeLists.txt | 6 ------ gtsam/discrete/CMakeLists.txt | 6 ------ gtsam/geometry/CMakeLists.txt | 6 ------ gtsam/inference/CMakeLists.txt | 6 ------ gtsam/linear/CMakeLists.txt | 5 ----- gtsam/navigation/CMakeLists.txt | 5 ----- gtsam/nonlinear/CMakeLists.txt | 6 ------ gtsam/slam/CMakeLists.txt | 5 ----- gtsam/symbolic/CMakeLists.txt | 6 ------ gtsam_unstable/CMakeLists.txt | 7 ++++--- gtsam_unstable/base/CMakeLists.txt | 5 ----- gtsam_unstable/timing/CMakeLists.txt | 1 + .../tests => timing}/timeDSFvariants.cpp | 0 .../tests => timing}/timeDSFvariants.xlsx | Bin .../tests => timing}/timeDSFvariants2.xlsx | Bin .../timeInertialNavFactor_GlobalVelocity.cpp | 18 +++++++++--------- tests/CMakeLists.txt | 13 ------------- timing/CMakeLists.txt | 6 ++++++ {tests => timing}/timeBatch.cpp | 0 .../tests => timing}/timeCalibratedCamera.cpp | 4 ++-- .../tests => timing}/timeFactorOverhead.cpp | 0 .../tests => timing}/timeGaussianFactor.cpp | 13 +++++++------ {tests => timing}/timeGaussianFactorGraph.cpp | 18 +++++++----------- {tests => timing}/timeIncremental.cpp | 0 {gtsam/slam/tests => timing}/timeLago.cpp | 0 {gtsam/base/tests => timing}/timeMatrix.cpp | 2 +- .../base/tests => timing}/timeMatrixOps.cpp | 0 .../tests => timing}/timePinholeCamera.cpp | 2 +- .../geometry/tests => timing}/timePose2.cpp | 2 +- .../geometry/tests => timing}/timePose3.cpp | 4 ++-- {gtsam/geometry/tests => timing}/timeRot2.cpp | 0 {gtsam/geometry/tests => timing}/timeRot3.cpp | 0 .../linear/tests => timing}/timeSLAMlike.cpp | 0 .../tests => timing}/timeStereoCamera.cpp | 2 +- {gtsam/base/tests => timing}/timeTest.cpp | 0 {gtsam/base/tests => timing}/timeVirtual.cpp | 0 {gtsam/base/tests => timing}/timeVirtual2.cpp | 0 {tests => timing}/timeiSAM2Chain.cpp | 0 39 files changed, 45 insertions(+), 106 deletions(-) create mode 100644 gtsam_unstable/timing/CMakeLists.txt rename gtsam_unstable/{base/tests => timing}/timeDSFvariants.cpp (100%) rename gtsam_unstable/{base/tests => timing}/timeDSFvariants.xlsx (100%) rename gtsam_unstable/{base/tests => timing}/timeDSFvariants2.xlsx (100%) rename gtsam_unstable/{slam/tests => timing}/timeInertialNavFactor_GlobalVelocity.cpp (86%) create mode 100644 timing/CMakeLists.txt rename {tests => timing}/timeBatch.cpp (100%) rename {gtsam/geometry/tests => timing}/timeCalibratedCamera.cpp (96%) rename {gtsam/linear/tests => timing}/timeFactorOverhead.cpp (100%) rename {gtsam/linear/tests => timing}/timeGaussianFactor.cpp (92%) rename {tests => timing}/timeGaussianFactorGraph.cpp (91%) rename {tests => timing}/timeIncremental.cpp (100%) rename {gtsam/slam/tests => timing}/timeLago.cpp (100%) rename {gtsam/base/tests => timing}/timeMatrix.cpp (99%) rename {gtsam/base/tests => timing}/timeMatrixOps.cpp (100%) rename {gtsam/geometry/tests => timing}/timePinholeCamera.cpp (98%) rename {gtsam/geometry/tests => timing}/timePose2.cpp (99%) rename {gtsam/geometry/tests => timing}/timePose3.cpp (90%) rename {gtsam/geometry/tests => timing}/timeRot2.cpp (100%) rename {gtsam/geometry/tests => timing}/timeRot3.cpp (100%) rename {gtsam/linear/tests => timing}/timeSLAMlike.cpp (100%) rename {gtsam/geometry/tests => timing}/timeStereoCamera.cpp (96%) rename {gtsam/base/tests => timing}/timeTest.cpp (100%) rename {gtsam/base/tests => timing}/timeVirtual.cpp (100%) rename {gtsam/base/tests => timing}/timeVirtual2.cpp (100%) rename {tests => timing}/timeiSAM2Chain.cpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 010d4de65..d64814829 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -303,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) 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/navigation/CMakeLists.txt b/gtsam/navigation/CMakeLists.txt index 3e82af774..e2b2fdce6 100644 --- a/gtsam/navigation/CMakeLists.txt +++ b/gtsam/navigation/CMakeLists.txt @@ -4,8 +4,3 @@ install(FILES ${navigation_headers} DESTINATION include/gtsam/navigation) # Add all tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(navigation "gtsam" "gtsam" "${navigation_excluded_files}") -endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index b4d288104..5bb2bd7d2 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -4,9 +4,3 @@ install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(nonlinear "gtsam" "gtsam" "${nonlinear_excluded_files}") -endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt index b348c4aa3..22645973d 100644 --- a/gtsam/slam/CMakeLists.txt +++ b/gtsam/slam/CMakeLists.txt @@ -8,8 +8,3 @@ install(FILES ${slam_headers} DESTINATION include/gtsam/slam) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(slam "gtsam" "gtsam" "${slam_excluded_files}") -endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/symbolic/CMakeLists.txt b/gtsam/symbolic/CMakeLists.txt index 6535755f8..feb073f69 100644 --- a/gtsam/symbolic/CMakeLists.txt +++ b/gtsam/symbolic/CMakeLists.txt @@ -4,9 +4,3 @@ install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(symbolic "gtsam" "gtsam" "${symbolic_excluded_files}") -endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index a33a4548c..6acb51ff6 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -118,6 +118,7 @@ endif(GTSAM_INSTALL_MATLAB_TOOLBOX) # Build examples -if (GTSAM_BUILD_EXAMPLES) - add_subdirectory(examples) -endif(GTSAM_BUILD_EXAMPLES) +add_subdirectory(examples) + +# Build timing +add_subdirectory(timing) diff --git a/gtsam_unstable/base/CMakeLists.txt b/gtsam_unstable/base/CMakeLists.txt index d83e97d26..2cb96be36 100644 --- a/gtsam_unstable/base/CMakeLists.txt +++ b/gtsam_unstable/base/CMakeLists.txt @@ -4,8 +4,3 @@ install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base) # Add all tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(base_unstable "${base_full_libs}" "${base_full_libs}" "${base_excluded_files}") -endif(GTSAM_BUILD_TIMING) diff --git a/gtsam_unstable/timing/CMakeLists.txt b/gtsam_unstable/timing/CMakeLists.txt new file mode 100644 index 000000000..b5130ae92 --- /dev/null +++ b/gtsam_unstable/timing/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTimingGlob("*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/base/tests/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp similarity index 100% rename from gtsam_unstable/base/tests/timeDSFvariants.cpp rename to gtsam_unstable/timing/timeDSFvariants.cpp diff --git a/gtsam_unstable/base/tests/timeDSFvariants.xlsx b/gtsam_unstable/timing/timeDSFvariants.xlsx similarity index 100% rename from gtsam_unstable/base/tests/timeDSFvariants.xlsx rename to gtsam_unstable/timing/timeDSFvariants.xlsx diff --git a/gtsam_unstable/base/tests/timeDSFvariants2.xlsx b/gtsam_unstable/timing/timeDSFvariants2.xlsx similarity index 100% rename from gtsam_unstable/base/tests/timeDSFvariants2.xlsx rename to gtsam_unstable/timing/timeDSFvariants2.xlsx diff --git a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp similarity index 86% rename from gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp rename to gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index 5132df658..0d514abcd 100644 --- a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -20,9 +20,9 @@ #include #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..1040c7ae1 --- /dev/null +++ b/timing/CMakeLists.txt @@ -0,0 +1,6 @@ +list(APPEND to_exclude + timeFactorOverhead.cpp + timeSLAMlike.cpp) +gtsamAddTimingGlob("*.cpp" "${to_exclude}" "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 100% rename from gtsam/linear/tests/timeFactorOverhead.cpp rename to timing/timeFactorOverhead.cpp 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/linear/tests/timeSLAMlike.cpp b/timing/timeSLAMlike.cpp similarity index 100% rename from gtsam/linear/tests/timeSLAMlike.cpp rename to timing/timeSLAMlike.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 From f5b3c75133381560940d65ec691b04cf9c12ec50 Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Fri, 20 Jun 2014 14:28:18 -0400 Subject: [PATCH 04/15] Fixes for windows Seems like a default constructor for KeyInfoEntry() is needed --- gtsam/linear/IterativeSolver.h | 1 + gtsam/linear/SubgraphPreconditioner.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index a7787d15a..bf11b3986 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -104,6 +104,7 @@ namespace gtsam { class GTSAM_EXPORT KeyInfoEntry : public boost::tuple { public: typedef boost::tuple Base; + KeyInfoEntry(){} KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} const size_t index() const { return this->get<0>(); } const size_t dim() const { return this->get<1>(); } diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 4c65d14ca..5f8b5ade1 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -158,7 +158,7 @@ namespace gtsam { SubgraphBuilderParameters parameters_; private: - SubgraphBuilder() {} + SubgraphBuilder() {}; }; /*******************************************************************************************/ From 921a18ae272c002199ccd1db8fcf3d593b5377cf Mon Sep 17 00:00:00 2001 From: balderdash-devil Date: Fri, 20 Jun 2014 18:43:08 -0400 Subject: [PATCH 05/15] fixed windows build issues with chris --- gtsam/linear/IterativeSolver.h | 2 +- gtsam/linear/SubgraphPreconditioner.h | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index bf11b3986..3c397c9e9 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -104,7 +104,7 @@ namespace gtsam { class GTSAM_EXPORT KeyInfoEntry : public boost::tuple { public: typedef boost::tuple Base; - KeyInfoEntry(){} + KeyInfoEntry(){} KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} const size_t index() const { return this->get<0>(); } const size_t dim() const { return this->get<1>(); } diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 5f8b5ade1..86bee2188 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -157,8 +157,6 @@ namespace gtsam { Weights weights(const GaussianFactorGraph &gfg) const; SubgraphBuilderParameters parameters_; - private: - SubgraphBuilder() {}; }; /*******************************************************************************************/ From 873283c522a5b89a1a4a148169c15352b8b1559a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 21 Jun 2014 16:26:48 -0700 Subject: [PATCH 06/15] Fixed broken timing script and fixed logic for building/excluding examples and timing scripts --- CMakeLists.txt | 2 +- cmake/GtsamTesting.cmake | 18 ++--- timing/CMakeLists.txt | 5 +- timing/timeFactorOverhead.cpp | 53 +++++++------ timing/timeSLAMlike.cpp | 144 ---------------------------------- 5 files changed, 40 insertions(+), 182 deletions(-) delete mode 100644 timing/timeSLAMlike.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ca25e132..004ded5e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -363,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 6195dc510..2e505c11e 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -52,7 +52,7 @@ endmacro() # an empty string "" if nothing needs to be excluded. # linkLibraries: The list of libraries to link to. macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries) - gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" "${GTSAM_BUILD_EXAMPLES_ALWAYS}") + gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) endmacro() @@ -77,7 +77,7 @@ endmacro() # 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" "FALSE") + gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS}) endmacro() @@ -88,7 +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 "Enable/Disable building of timing scripts" OFF) # These do not currently work +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) @@ -107,9 +107,7 @@ endif() add_custom_target(examples) # Add timing target -if(GTSAM_BUILD_TIMING) - add_custom_target(timing) -endif() +add_custom_target(timing) # Include obsolete macros - will be removed in the near future include(GtsamTestingObsolete) @@ -258,10 +256,12 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b # Add TOPSRCDIR set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - - if(NOT buildWithAll) + + # 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) diff --git a/timing/CMakeLists.txt b/timing/CMakeLists.txt index 1040c7ae1..fc16d3ac8 100644 --- a/timing/CMakeLists.txt +++ b/timing/CMakeLists.txt @@ -1,6 +1,3 @@ -list(APPEND to_exclude - timeFactorOverhead.cpp - timeSLAMlike.cpp) -gtsamAddTimingGlob("*.cpp" "${to_exclude}" "gtsam") +gtsamAddTimingGlob("*.cpp" "" "gtsam") target_link_libraries(timeGaussianFactorGraph CppUnitLite) diff --git a/timing/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp index 6e5d6b9ad..97c40e6f4 100644 --- a/timing/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/timing/timeSLAMlike.cpp b/timing/timeSLAMlike.cpp deleted file mode 100644 index 479c03940..000000000 --- a/timing/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 Date: Sun, 22 Jun 2014 18:39:11 -0400 Subject: [PATCH 07/15] Simplified this example a bit --- .cproject | 348 +++++++++++++++------------- examples/SFMExample_SmartFactor.cpp | 82 +++---- 2 files changed, 219 insertions(+), 211 deletions(-) diff --git a/.cproject b/.cproject index 1783efa98..2f42abd79 100644 --- a/.cproject +++ b/.cproject @@ -542,6 +542,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -568,6 +576,7 @@ make + tests/testBayesTree.run true false @@ -575,6 +584,7 @@ make + testBinaryBayesNet.run true false @@ -622,6 +632,7 @@ make + testSymbolicBayesNet.run true false @@ -629,6 +640,7 @@ make + tests/testSymbolicFactor.run true false @@ -636,6 +648,7 @@ make + testSymbolicFactorGraph.run true false @@ -651,19 +664,12 @@ make + tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -896,22 +902,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -928,6 +918,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -1458,6 +1464,7 @@ make + testGraph.run true false @@ -1465,6 +1472,7 @@ make + testJunctionTree.run true false @@ -1472,6 +1480,7 @@ make + testSymbolicBayesNetB.run true false @@ -1639,6 +1648,7 @@ make + testErrors.run true false @@ -1684,22 +1694,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1780,6 +1774,22 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1996,22 +2006,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2094,7 +2088,6 @@ make - testSimulated2DOriented.run true false @@ -2134,7 +2127,6 @@ make - testSimulated2D.run true false @@ -2142,7 +2134,6 @@ make - testSimulated3D.run true false @@ -2156,6 +2147,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2364,6 +2371,14 @@ true true + + make + -j5 + SFMExample_SmartFactor.run + true + true + true + make -j5 @@ -2430,7 +2445,6 @@ make - tests/testGaussianISAM2 true false @@ -2452,102 +2466,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2749,6 +2667,7 @@ cpack + -G DEB true false @@ -2756,6 +2675,7 @@ cpack + -G RPM true false @@ -2763,6 +2683,7 @@ cpack + -G TGZ true false @@ -2770,6 +2691,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2943,26 +2865,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - check.wrap + -j2 + testRot2.run true true true - + make - -j5 - wrap + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -3006,6 +3000,30 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 904919d42..400ddd728 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -14,6 +14,7 @@ * @brief A structure-from-motion problem on a simulated dataset, using smart projection factor * @author Duy-Nguyen Ta * @author Jing Dong + * @author Frank Dellaert */ /** @@ -28,11 +29,6 @@ // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). #include -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols -#include - // In GTSAM, measurement functions are represented as 'factors'. // The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, // The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, @@ -65,8 +61,8 @@ using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor - SmartFactor; +typedef gtsam::SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -75,93 +71,87 @@ int main(int argc, char* argv[]) { Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - // Create the set of ground-truth landmarks + // Create the set of ground-truth landmarks and poses vector points = createPoints(); - - // Create the set of ground-truth poses vector poses = createPoses(); // Create a factor graph NonlinearFactorGraph graph; - // A vector saved all Smart factors (for get landmark position after optimization) - vector smartfactors_ptr; - // Simulated measurements from each camera pose, adding them to the factor graph - for (size_t i = 0; i < points.size(); ++i) { + for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. SmartFactor::shared_ptr smartfactor(new SmartFactor()); - for (size_t j = 0; j < poses.size(); ++j) { + for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[j], *K); - Point2 measurement = camera.project(points[i]); + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); - // call add() function to add measurment into a single factor, here we need to add: + // call add() function to add measurement into a single factor, here we need to add: // 1. the 2D measurement // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, Symbol('x', j), measurementNoise, K); + smartfactor->add(measurement, i, measurementNoise, K); } - // save smartfactors to get landmark position - smartfactors_ptr.push_back(smartfactor); - // insert the smart factor in the graph graph.push_back(smartfactor); } // Add a prior on pose x0. This indirectly specifies where the origin is. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); + graph.push_back(PriorFactor(0, poses[0], poseNoise)); - // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained - // Here we add a prior on the second pose x1, so this will fix the scale by indicating the distance between x0 and x1. - // Because these two are fixed, the rest poses will be alse fixed. - graph.push_back(PriorFactor(Symbol('x', 1), poses[1], poseNoise)); // add directly to graph + // Because the structure-from-motion problem has a scale ambiguity, the problem is + // still under-constrained. Here we add a prior on the second pose x1, so this will + // fix the scale by indicating the distance between x0 and x1. + // Because these two are fixed, the rest of the poses will be also be fixed. + graph.push_back(PriorFactor(1, poses[1], poseNoise)); // add directly to graph graph.print("Factor Graph:\n"); - // Create the data structure to hold the initial estimate to the solution + // Create the initial estimate to the solution // Intentionally initialize the variables off from the ground truth Values initialEstimate; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initialEstimate.insert(i, poses[i].compose(delta)); initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results Values result = DoglegOptimizer(graph, initialEstimate).optimize(); result.print("Final results:\n"); - - // Notice: Smart factor represent the 3D point as a factor, not a variable. + // A smart factor represent the 3D point inside the factor, not as a variable. // The 3D position of the landmark is not explicitly calculated by the optimizer. - // If you do want to output the landmark's 3D position, you should use the internal function point() - // of the smart factor to get the 3D point. + // To obtain the landmark's 3D position, we use the "point" method of the smart factor. Values landmark_result; - for (size_t i = 0; i < points.size(); ++i) { + for (size_t j = 0; j < points.size(); ++j) { - // The output of point() is in boost::optional, since sometimes - // the triangulation opterations inside smart factor will encounter degeneracy. - // Check the manual of boost::optional for more details for the usages. + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. boost::optional point; - // here we use the saved smart factors to call, pass in all optimized pose to calculate landmark positions - point = smartfactors_ptr.at(i)->point(result); - - // ignore if boost::optional return NULL - if (point) - landmark_result.insert(Symbol('l', i), *point); + // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first + SmartFactor::shared_ptr smart = dynamic_pointer_cast(graph[j]); + if (smart) { + point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } } landmark_result.print("Landmark results:\n"); - return 0; } /* ************************************************************************* */ From d8a8d459ae8b56e107f4da3a0143ba034ffae7cf Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Jun 2014 18:51:35 -0400 Subject: [PATCH 08/15] Moved test and timing targets to their correct locations --- .cproject | 2166 +++++++++++++++++++++++++++-------------------------- 1 file changed, 1087 insertions(+), 1079 deletions(-) diff --git a/.cproject b/.cproject index 2f42abd79..62330cdbc 100644 --- a/.cproject +++ b/.cproject @@ -518,6 +518,22 @@ true true + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + make -j2 @@ -542,14 +558,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -670,238 +678,6 @@ false true - - make - -j5 - testPoseRTV.run - true - true - true - - - make - -j5 - testIMUSystem.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - make -j2 @@ -918,46 +694,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - make -j2 @@ -982,90 +718,74 @@ true true - + make -j5 - schedulingExample.run + testAHRS.run true true true - + make -j5 - testCSP.run + testImuFactor.run true true true - + make -j5 - testScheduler.run + testInvDepthFactor3.run true true true - + make -j5 - schedulingQuals12.run + testMultiProjectionFactor.run true true true - + make -j5 - testSudoku.run + testPoseRotationPrior.run true true true - + make -j5 - schedulingQuals13.run + testPoseTranslationPrior.run true true true - + make -j5 - testWrap.run + testReferenceFrameFactor.run true true true - + make - -j2 - check + -j5 + testSmartProjectionFactor.run true true true - + make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean + -j5 + testTSAMFactors.run true true true @@ -1166,382 +886,22 @@ true true - + make -j5 - testInvDepthFactor3.run + testCombinedImuFactor.run true true true - + make -j5 - testPoseTranslationPrior.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testReferenceFrameFactor.run - true - true - true - - - make - -j5 - testAHRS.run - true - true - true - - - make - -j8 testImuFactor.run true true true - - make - -j5 - testMultiProjectionFactor.run - true - true - true - - - make - -j5 - testSmartProjectionFactor.run - true - true - true - - - make - -j5 - testTSAMFactors.run - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - testJunctionTree.run - true - false - true - - - make - - testSymbolicBayesNetB.run - true - false - true - - - make - -j5 - testGaussianISAM.run - true - true - true - - - make - -j5 - testDoglegOptimizer.run - true - true - true - - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - make -j5 @@ -1774,22 +1134,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1886,54 +1230,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -1942,66 +1238,42 @@ true true - + make -j5 - testAntiFactor.run + testBTree.run true true true - + make -j5 - testBetweenFactor.run + testDSF.run true true true - + make -j5 - testDataset.run + testDSFMap.run true true true - + make -j5 - testEssentialMatrixFactor.run + testDSFVector.run true true true - + make -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run + testFixedVector.run true true true @@ -2147,22 +1419,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j5 @@ -2203,253 +1459,6 @@ false true - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j5 - Pose2SLAMExample_lago.run - true - true - true - - - make - -j5 - Pose2SLAMExample_g2o.run - true - true - true - - - make - -j5 - SFMExample_SmartFactor.run - true - true - true - - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -2865,6 +1874,1044 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testPoseRTV.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + schedulingExample.run + true + true + true + + + make + -j5 + schedulingQuals12.run + true + true + true + + + make + -j5 + schedulingQuals13.run + true + true + true + + + make + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + testSudoku.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + + testGraph.run + true + false + true + + + make + + testJunctionTree.run + true + false + true + + + make + + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + SFMExample_SmartFactor.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -2961,45 +3008,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - make -j5 From c93c9f1863eceff1df8369eba67693dbf99751ea Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sun, 22 Jun 2014 23:52:59 -0400 Subject: [PATCH 09/15] fix namespace --- examples/SFMExample_SmartFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 400ddd728..b3c5ee5fe 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -142,7 +142,7 @@ int main(int argc, char* argv[]) { boost::optional point; // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first - SmartFactor::shared_ptr smart = dynamic_pointer_cast(graph[j]); + SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { point = smart->point(result); if (point) // ignore if boost::optional return NULL From b698e30acbe21e3235c30814947240668402e331 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 24 Jun 2014 23:30:30 -0400 Subject: [PATCH 10/15] Small speedup in project (result of Visual SLAM slides :-)) --- gtsam/geometry/PinholeCamera.h | 7 ++++--- timing/timePinholeCamera.cpp | 10 ++++++++++ 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 19eb87b5f..709b95181 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -275,11 +275,12 @@ public: if (P.z() <= 0) throw CheiralityException(); #endif + double d = 1.0 / P.z(); + const double u = P.x() * d, v = P.y() * d; if (Dpoint) { - double d = 1.0 / P.z(), d2 = d * d; - *Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); + *Dpoint = (Matrix(2, 3) << d, 0.0, -u * d, 0.0, d, -v * d); } - return Point2(P.x() / P.z(), P.y() / P.z()); + return Point2(u, v); } /// Project a point into the image and check depth diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index 66a8ca099..8e3e12e22 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -53,6 +53,10 @@ int main() // After Cal3DS2 fix: 0.12231 musecs/call // Cal3Bundler: 0.12000 musecs/call // Cal3Bundler fix: 0.14637 musecs/call + // June 24 2014, Macbook Pro 2.3GHz Core i7 + // GTSAM 3.1: 0.04295 musecs/call + // After project fix: 0.04193 musecs/call + { long timeLog = clock(); for(int i = 0; i < n; i++) @@ -70,6 +74,9 @@ int main() // After Cal3DS2 fix: 3.2857 musecs/call // Cal3Bundler: 2.6556 musecs/call // Cal3Bundler fix: 2.1613 musecs/call + // June 24 2014, Macbook Pro 2.3GHz Core i7 + // GTSAM 3.1: 0.2322 musecs/call + // After project fix: 0.2094 musecs/call { Matrix Dpose, Dpoint; long timeLog = clock(); @@ -88,6 +95,9 @@ int main() // After Cal3DS2 fix: 3.4483 musecs/call // Cal3Bundler: 2.5112 musecs/call // Cal3Bundler fix: 2.0946 musecs/call + // June 24 2014, Macbook Pro 2.3GHz Core i7 + // GTSAM 3.1: 0.2294 musecs/call + // After project fix: 0.2093 musecs/call { Matrix Dpose, Dpoint, Dcal; long timeLog = clock(); From 1a9ab972a9a8ac14ceb26e4498bc65cb1a8bd2fa Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sun, 29 Jun 2014 13:08:17 -0400 Subject: [PATCH 11/15] make verbosityTranslator public static functions --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 4 ++-- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 5 ++--- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 4 ++-- gtsam/nonlinear/NonlinearOptimizerParams.h | 5 ++--- 4 files changed, 8 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 08961db86..002f8b237 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -37,7 +37,7 @@ using boost::adaptors::map_values; /* ************************************************************************* */ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator( - const std::string &src) const { + const std::string &src) { std::string s = src; boost::algorithm::to_upper(s); if (s == "SILENT") @@ -59,7 +59,7 @@ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTrans /* ************************************************************************* */ std::string LevenbergMarquardtParams::verbosityLMTranslator( - VerbosityLM value) const { + VerbosityLM value) { std::string s; switch (value) { case LevenbergMarquardtParams::SILENT: diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 5e8e3923c..20f9dec3c 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -41,9 +41,8 @@ public: SILENT = 0, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA }; -private: - VerbosityLM verbosityLMTranslator(const std::string &s) const; - std::string verbosityLMTranslator(VerbosityLM value) const; + static VerbosityLM verbosityLMTranslator(const std::string &s); + static std::string verbosityLMTranslator(VerbosityLM value); public: diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index b7ad342ca..ef0f2aa9b 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -14,7 +14,7 @@ namespace gtsam { /* ************************************************************************* */ NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslator( - const std::string &src) const { + const std::string &src) { std::string s = src; boost::algorithm::to_upper(s); if (s == "SILENT") @@ -36,7 +36,7 @@ NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslato /* ************************************************************************* */ std::string NonlinearOptimizerParams::verbosityTranslator( - Verbosity value) const { + Verbosity value) { std::string s; switch (value) { case NonlinearOptimizerParams::SILENT: diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index cd5496209..dafc1f065 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -84,9 +84,8 @@ public: verbosity = verbosityTranslator(src); } -private: - Verbosity verbosityTranslator(const std::string &s) const; - std::string verbosityTranslator(Verbosity value) const; + static Verbosity verbosityTranslator(const std::string &s) ; + static std::string verbosityTranslator(Verbosity value) ; // Successive Linearization Parameters From 991906181b0d38cd36234deeede8abfe0d38d92b Mon Sep 17 00:00:00 2001 From: Vadim Indelman Date: Mon, 30 Jun 2014 20:43:25 +0300 Subject: [PATCH 12/15] Updating EM factor (to accout for uncertainty in states) --- gtsam_unstable/gtsam_unstable.h | 4 ++ gtsam_unstable/slam/BetweenFactorEM.h | 72 +++++++++++++++++++ .../slam/tests/testBetweenFactorEM.cpp | 45 ++++++++++++ 3 files changed, 121 insertions(+) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 06a41f5ec..66b761cbc 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -332,6 +332,10 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor { void set_flag_bump_up_near_zero_probs(bool flag); bool get_flag_bump_up_near_zero_probs() const; + void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph); + Matrix get_model_inlier_cov(); + Matrix get_model_outlier_cov(); + void serializable() const; // enabling serialization functionality }; diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 234ad25bc..9941aaf74 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace gtsam { @@ -292,6 +293,77 @@ namespace gtsam { return flag_bump_up_near_zero_probs_; } + /* ************************************************************************* */ + SharedGaussian get_model_inlier() const { + return model_inlier_; + } + + /* ************************************************************************* */ + SharedGaussian get_model_outlier() const { + return model_outlier_; + } + + /* ************************************************************************* */ + Matrix get_model_inlier_cov() const { + return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + } + + /* ************************************************************************* */ + Matrix get_model_outlier_cov() const { + return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + } + + /* ************************************************************************* */ + void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ + + const T& p1 = values.at(key1_); + const T& p2 = values.at(key2_); + + Matrix H1, H2; + T hx = p1.between(p2, H1, H2); // h(x) + + // get joint covariance of the involved states + std::vector Keys; + Keys.push_back(key1_); + Keys.push_back(key2_); + Marginals marginals( graph, values, Marginals::QR ); + JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); + Matrix cov1 = joint_marginal12(key1_, key1_); + Matrix cov2 = joint_marginal12(key2_, key2_); + Matrix cov12 = joint_marginal12(key1_, key2_); + + Matrix H; + H.resize(H1.rows(), H1.rows()+H2.rows()); + H << H1, H2; // H = [H1 H2] + + Matrix joint_cov; + joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); + joint_cov << cov1, cov12, + cov12.transpose(), cov2; + + Matrix cov_state = H*joint_cov*H.transpose(); + + // model_inlier_->print("before:"); + + // update inlier and outlier noise models + Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); + + Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); + + // model_inlier_->print("after:"); + // std::cout<<"covRinlier + cov_state: "< #include +#include using namespace std; using namespace gtsam; + // Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below // to reenable the test. #if 0 @@ -251,6 +253,49 @@ TEST( BetweenFactorEM, CaseStudy) } } + +///* ************************************************************************** */ +TEST (BetweenFactorEM, updateNoiseModel ) { + gtsam::Key key1(1); + gtsam::Key key2(2); + + gtsam::Pose2 p1(10.0, 15.0, 0.1); + gtsam::Pose2 p2(15.0, 15.0, 0.3); + gtsam::Pose2 noise(0.5, 0.4, 0.01); + gtsam::Pose2 rel_pose_ideal = p1.between(p2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0))); + + gtsam::Values values; + values.insert(key1, p1); + values.insert(key2, p2); + + double prior_outlier = 0.0; + double prior_inlier = 1.0; + + BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); + + SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); + + NonlinearFactorGraph graph; + graph.push_back(gtsam::PriorFactor(key1, p1, model)); + graph.push_back(gtsam::PriorFactor(key2, p2, model)); + + f.updateNoiseModels(values, graph); + + SharedGaussian model_inlier_new = f.get_model_inlier(); + SharedGaussian model_outlier_new = f.get_model_outlier(); + + model_inlier->print("model_inlier:"); + model_outlier->print("model_outlier:"); + model_inlier_new->print("model_inlier_new:"); + model_outlier_new->print("model_outlier_new:"); +} + + #endif /* ************************************************************************* */ From 87ba9384e0774551814ef4ee6d734f9ffa70ad4e Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 1 Jul 2014 11:21:23 -0400 Subject: [PATCH 13/15] warning in release --- examples/SFMExample_bal.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 9d453fe5f..43177dc42 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -43,8 +43,7 @@ int main (int argc, char* argv[]) { // Load the SfM data from file SfM_data mydata; - const bool success = readBAL(filename, mydata); - assert(success); + assert(readBAL(filename, mydata)); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph From d6ffef89e6d23c884e0925819353ffa51769820e Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 1 Jul 2014 11:21:43 -0400 Subject: [PATCH 14/15] Return pointer to cached basis --- gtsam/geometry/Unit3.cpp | 2 +- gtsam/geometry/Unit3.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index db4c8da83..ad6a41bde 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -58,7 +58,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -Matrix Unit3::basis() const { +const Matrix& Unit3::basis() const { // Return cached version if exists if (B_.rows() == 3) diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 39e2c9799..8d2c024c0 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -84,7 +84,7 @@ public: * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions * tangent to the sphere at the current direction. */ - Matrix basis() const; + const Matrix& basis() const; /// Return skew-symmetric associated with 3D point on unit sphere Matrix skew() const; From c8c25b16abf10f58d6f1da1f2fa30a9692f9885f Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 1 Jul 2014 11:21:58 -0400 Subject: [PATCH 15/15] Added epipoles --- gtsam/geometry/EssentialMatrix.h | 10 ++++++ gtsam/geometry/tests/testEssentialMatrix.cpp | 33 ++++++++++++++++++-- 2 files changed, 41 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 3c9568c3d..32b966261 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -112,6 +112,16 @@ public: return E_; } + /// Return epipole in image_a , as Unit3 to allow for infinity + inline const Unit3& epipole_a() const { + return aTb_; // == direction() + } + + /// Return epipole in image_b, as Unit3 to allow for infinity + inline Unit3 epipole_b() const { + return aRb_.unrotate(aTb_); // == rotation.unrotate(direction()) + } + /** * @brief takes point in world coordinates and transforms it to pose with |t|==1 * @param p point in world coordinates diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 9ad30bc41..4d34bbe3d 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -144,9 +144,9 @@ TEST (EssentialMatrix, FromPose3_b) { Matrix actualH; Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); - EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); + EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras - EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); EXPECT(assert_equal(expectedH, actualH, 1e-5)); @@ -161,6 +161,35 @@ TEST (EssentialMatrix, streaming) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, epipoles) { + // Create an E + Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Point3 c1Tc2(0.4, 0.5, 0.6); + EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); + + // Calculate expected values through SVD + Matrix U, V; + Vector S; + gtsam::svd(E.matrix(), U, S, V); + + // check rank 2 constraint + CHECK(fabs(S(2))<1e-10); + + // check epipoles not at infinity + CHECK(fabs(U(2,2))>1e-10 && fabs(V(2,2))>1e-10); + + // Check epipoles + + // Epipole in image 1 is just E.direction() + Unit3 e1(U(0, 2), U(1, 2), U(2, 2)); + EXPECT(assert_equal(e1, E.epipole_a())); + + // Epipole in image 2 is E.rotation().unrotate(E.direction()) + Unit3 e2(V(0, 2), V(1, 2), V(2, 2)); + EXPECT(assert_equal(e2, E.epipole_b())); +} + /* ************************************************************************* */ int main() { TestResult tr;