diff --git a/.cproject b/.cproject index b76463977..1282db639 100644 --- a/.cproject +++ b/.cproject @@ -732,46 +732,6 @@ true true - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run - true - true - true - make -j2 @@ -2015,6 +1975,134 @@ 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 -j2 @@ -2095,70 +2183,6 @@ true true - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j2 @@ -2191,6 +2215,14 @@ true true + + make + -j5 + testWrap.run + true + true + true + make -j5 @@ -2263,6 +2295,22 @@ true true + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + make -j5 @@ -2535,146 +2583,66 @@ true true - + make -j5 - testStereoCamera.run + testAntiFactor.run true true true - + make -j5 - testRot3M.run + testBetweenFactor.run true true true - + make -j5 - testPoint3.run + testDataset.run true true true - + make -j5 - testCalibratedCamera.run + testEssentialMatrixFactor.run true true true - + make -j5 - timeStereoCamera.run + testGeneralSFMFactor_Cal3Bundler.run true true true - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - + make -j5 - testPoint2.run + testGeneralSFMFactor.run true true true - + make -j5 - testPose2.run + testProjectionFactor.run true true true - + make -j5 - testPose3.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run + testRotateFactor.run true true true @@ -2831,6 +2799,70 @@ true true + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.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 @@ -2958,14 +2990,6 @@ true true - - make - -j5 - testWrap.run - true - true - true - make -j5 diff --git a/.gitignore b/.gitignore index 6d274af09..cf23a5147 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,6 @@ /build* /doc* *.pyc -*.DS_Store \ No newline at end of file +*.DS_Store +/examples/Data/dubrovnik-3-7-pre-rewritten.txt +/examples/Data/pose2example-rewritten.txt \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index d03f11ede..933f2083e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.6) # Set the version number for the library set (GTSAM_VERSION_MAJOR 3) -set (GTSAM_VERSION_MINOR 0) +set (GTSAM_VERSION_MINOR 1) set (GTSAM_VERSION_PATCH 0) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") @@ -91,10 +91,10 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") # If using Boost shared libs, disable auto linking if(MSVC) # Some libraries, at least Boost Program Options, rely on this to export DLL symbols - add_definitions(-DBOOST_ALL_DYN_LINK) # Disable autolinking if(NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_NO_LIB) + add_definitions(-DBOOST_ALL_DYN_LINK) endif() endif() @@ -273,6 +273,13 @@ if(MSVC) add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings endif() +# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8) + add_definitions(-Wno-unused-local-typedefs) + endif() +endif() + if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) endif() diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index ee2bbd42a..9a0b297ab 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -56,7 +56,7 @@ endif() # Clang on Mac uses a template depth that is less than standard and is too small if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") - add_definitions(-ftemplate-depth=1024) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") endif() endif() diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index 98bd7f469..5d018376b 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -93,7 +93,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex # Paths for generated files set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") - set(generated_cpp_file "${PROJECT_BINARY_DIR}/wrap/${moduleName}/${moduleName}_wrapper.cpp") + set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp") set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex") message(STATUS "Building wrap module ${moduleName}") @@ -108,24 +108,87 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex list(GET GTSAM_INCLUDE_DIR 0 installed_includes_path) set(matlab_h_path "${installed_includes_path}/wrap") endif() - - # Add -shared or -static suffix to targets + + # If building a static mex module, add all cmake-linked libraries to the + # explicit link libraries list so that the next block of code can unpack + # any static libraries + set(automaticDependencies "") + foreach(lib ${moduleName} ${linkLibraries}) + #message("MODULE NAME: ${moduleName}") + if(TARGET "${lib}") + get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES) + # message("DEPENDENT LIBRARIES: ${dependentLibraries}") + if(dependentLibraries) + list(APPEND automaticDependencies ${dependentLibraries}) + endif() + endif() + endforeach() + + ## CHRIS: Temporary fix. On my system the get_target_property above returned Not-found for gtsam module + ## This needs to be fixed!! + if(UNIX AND NOT APPLE) + list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE} + ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE} + ${Boost_REGEX_LIBRARY_RELEASE}) + if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0 + list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE}) + if(GTSAM_MEX_BUILD_STATIC_MODULE) + #list(APPEND automaticDependencies -Wl,--no-as-needed -lrt) + endif() + endif() + endif() + + #message("AUTOMATIC DEPENDENCIES: ${automaticDependencies}") + ## CHRIS: End temporary fix + + # Separate dependencies set(correctedOtherLibraries "") set(otherLibraryTargets "") set(otherLibraryNontargets "") - foreach(lib ${moduleName} ${linkLibraries}) - if(TARGET ${lib}) - list(APPEND correctedOtherLibraries ${lib}) - list(APPEND otherLibraryTargets ${lib}) - elseif(TARGET ${lib}-shared) # Prefer the shared library if we have both shared and static) - list(APPEND correctedOtherLibraries ${lib}-shared) - list(APPEND otherLibraryTargets ${lib}-shared) - elseif(TARGET ${lib}-static) - list(APPEND correctedOtherLibraries ${lib}-static) - list(APPEND otherLibraryTargets ${lib}-static) + set(otherSourcesAndObjects "") + foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies}) + if(TARGET "${lib}") + if(GTSAM_MEX_BUILD_STATIC_MODULE) + get_target_property(target_sources ${lib} SOURCES) + list(APPEND otherSourcesAndObjects ${target_sources}) + else() + list(APPEND correctedOtherLibraries ${lib}) + list(APPEND otherLibraryTargets ${lib}) + endif() else() - list(APPEND correctedOtherLibraries ${lib}) - list(APPEND otherLibraryNontargets ${lib}) + get_filename_component(file_extension "${lib}" EXT) + get_filename_component(lib_name "${lib}" NAME_WE) + if(file_extension STREQUAL ".a" AND GTSAM_MEX_BUILD_STATIC_MODULE) + # For building a static MEX module, unpack the static library + # and compile its object files into our module + file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects") + execute_process(COMMAND ar -x "${lib}" + WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects" + RESULT_VARIABLE ar_result) + if(NOT ar_result EQUAL 0) + message(FATAL_ERROR "Failed extracting ${lib}") + endif() + + # Get list of object files + execute_process(COMMAND ar -t "${lib}" + OUTPUT_VARIABLE object_files + RESULT_VARIABLE ar_result) + if(NOT ar_result EQUAL 0) + message(FATAL_ERROR "Failed listing ${lib}") + endif() + + # Add directory to object files + string(REPLACE "\n" ";" object_files_list "${object_files}") + foreach(object_file ${object_files_list}) + get_filename_component(file_extension "${object_file}" EXT) + if(file_extension STREQUAL ".o") + list(APPEND otherSourcesAndObjects "${generated_files_path}/${lib_name}_objects/${object_file}") + endif() + endforeach() + else() + list(APPEND correctedOtherLibraries ${lib}) + list(APPEND otherLibraryNontargets ${lib}) + endif() endif() endforeach() @@ -144,7 +207,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex file(MAKE_DIRECTORY "${generated_files_path}") add_custom_command( OUTPUT ${generated_cpp_file} - DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} + DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects} COMMAND wrap ${modulePath} @@ -157,7 +220,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex # Set up building of mex module string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}") string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}") - add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader}) + add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects}) target_link_libraries(${moduleName}_wrapper ${correctedOtherLibraries}) set_target_properties(${moduleName}_wrapper PROPERTIES OUTPUT_NAME "${moduleName}_wrapper" diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt deleted file mode 100644 index 12c9f4db4..000000000 --- a/examples/Data/dubrovnik-3-7-pre-rewritten.txt +++ /dev/null @@ -1,80 +0,0 @@ -3 7 19 - -0 0 -385.989990234375 387.1199951171875 -1 0 -38.439998626708984375 492.1199951171875 -2 0 -667.91998291015625 123.1100006103515625 -0 1 383.8800048828125 -15.299989700317382812 -1 1 559.75 -106.15000152587890625 -0 2 591.54998779296875 136.44000244140625 -1 2 863.8599853515625 -23.469970703125 -2 2 494.720001220703125 112.51999664306640625 -0 3 592.5 125.75 -1 3 861.08001708984375 -35.219970703125 -2 3 498.540008544921875 101.55999755859375 -0 4 348.720001220703125 558.3800048828125 -1 4 776.030029296875 483.529998779296875 -2 4 7.7800288200378417969 326.350006103515625 -0 5 14.010009765625 96.420013427734375 -1 5 207.1300048828125 118.3600006103515625 -0 6 202.7599945068359375 340.989990234375 -1 6 543.18011474609375 294.80999755859375 -2 6 -58.419979095458984375 110.8300018310546875 - -0.29656188120312942935 --0.035318354384285870207 -0.31252101755032046793 -0.47230274932665988752 --0.3572340863744113415 --2.0517704282499575896 -1430.031982421875 --7.5572756941255647689e-08 -3.2377570134516087119e-14 - -0.28532097381985194184 --0.27699838370789808817 -0.048601169984112867206 --1.2598695987143850861 --0.049063798188844320869 --1.9586867140445654023 -1432.137451171875 --7.3171918302250560373e-08 -3.1759419042137054801e-14 - -0.057491325683772541433 -0.34853090049579965592 -0.47985129303736057116 -8.1963904289063389541 -6.5146840788718787252 --3.8392804395897406344 -1572.047119140625 --1.5962623223231275915e-08 --1.6507904730136101212e-14 - --11.317351620610928364 -3.3594874875767186673 --42.755222607849105998 - -4.2648515634753199066 --8.4629358700849355301 --22.252086323427270997 - -10.996977688149536689 --9.2123370180278048025 --29.206739014051372294 - -10.935342607054865383 --9.4338917557810741954 --29.112263909175499776 - -15.714024935401759819 -1.3745079651566265433 --59.286834979937104606 - --1.3624227800805182031 --4.1979357415396094666 --21.034430148188398846 - -6.7690173115899296974 --4.7352452433700786827 --53.605307875695892506 - diff --git a/examples/Data/noisyToyGraph.txt b/examples/Data/noisyToyGraph.txt new file mode 100644 index 000000000..747f8da1c --- /dev/null +++ b/examples/Data/noisyToyGraph.txt @@ -0,0 +1,9 @@ +VERTEX_SE2 0 0.000000 0.000000 0.000000 +VERTEX_SE2 1 0.774115 1.183389 1.576173 +VERTEX_SE2 2 -0.262420 2.047059 -3.127594 +VERTEX_SE2 3 -1.605649 0.993891 -1.561134 +EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 diff --git a/examples/Data/optimizedNoisyToyGraph.txt b/examples/Data/optimizedNoisyToyGraph.txt new file mode 100644 index 000000000..217a76fb1 --- /dev/null +++ b/examples/Data/optimizedNoisyToyGraph.txt @@ -0,0 +1,9 @@ +VERTEX_SE2 0 0.000000 -0.000000 0.000000 +VERTEX_SE2 1 0.955797 1.137643 1.543041 +VERTEX_SE2 2 0.129867 1.989651 3.201259 +VERTEX_SE2 3 -1.047715 0.933789 4.743682 +EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 diff --git a/examples/Data/orientationsNoisyToyGraph.txt b/examples/Data/orientationsNoisyToyGraph.txt new file mode 100644 index 000000000..ada372a8e --- /dev/null +++ b/examples/Data/orientationsNoisyToyGraph.txt @@ -0,0 +1,9 @@ +VERTEX_SE2 0 0.000000 0.000000 0.000000 +VERTEX_SE2 1 0.000000 0.000000 1.565449 +VERTEX_SE2 2 0.000000 0.000000 3.134143 +VERTEX_SE2 3 0.000000 0.000000 4.710123 +EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 diff --git a/examples/Data/pose2example.txt b/examples/Data/pose2example.txt new file mode 100644 index 000000000..c6c69c881 --- /dev/null +++ b/examples/Data/pose2example.txt @@ -0,0 +1,23 @@ +VERTEX_SE2 0 0.000000 0.000000 0.000000 +VERTEX_SE2 1 1.030390 0.011350 -0.081596 +VERTEX_SE2 2 2.036137 -0.129733 -0.301887 +VERTEX_SE2 3 3.015097 -0.442395 -0.345514 +VERTEX_SE2 4 3.343949 0.506678 1.214715 +VERTEX_SE2 5 3.684491 1.464049 1.183785 +VERTEX_SE2 6 4.064626 2.414783 1.176333 +VERTEX_SE2 7 4.429778 3.300180 1.259169 +VERTEX_SE2 8 4.128877 2.321481 -1.825391 +VERTEX_SE2 9 3.884653 1.327509 -1.953016 +VERTEX_SE2 10 3.531067 0.388263 -2.148934 +EDGE_SE2 0 1 1.030390 0.011350 -0.081596 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 1 2 1.013900 -0.058639 -0.220291 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 2 3 1.027650 -0.007456 -0.043627 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 3 4 -0.012016 1.004360 1.560229 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 4 5 1.016030 0.014565 -0.030930 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 5 6 1.023890 0.006808 -0.007452 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 7 8 -1.023820 -0.013668 -3.084560 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 8 9 1.023440 0.013984 -0.127624 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 9 10 1.003350 0.022250 -0.195918 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 5 9 0.033943 0.032439 3.073637 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 3 10 0.044020 0.988477 -1.553511 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699 \ No newline at end of file diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp new file mode 100644 index 000000000..3a4ee9cff --- /dev/null +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * 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 Pose2SLAMExample_g2o.cpp + * @brief A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the + * optimization. Output is written on a file, in g2o format + * Syntax for the script is ./Pose2SLAMExample_g2o input.g2o output.g2o + * @date May 15, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = "../../examples/Data/noisyToyGraph.txt"; + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = readG2o(g2oFile); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = *graph; + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonOptimizer optimizer(graphWithPrior, *initial); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + if (argc < 3) { + result.print("result"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(*graph, result, outputFile); + std::cout << "done! " << std::endl; + } + return 0; +} diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp new file mode 100644 index 000000000..1f5d80d7b --- /dev/null +++ b/examples/Pose2SLAMExample_lago.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * 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 Pose2SLAMExample_lago.cpp + * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem + * using LAGO (Linear Approximation for Graph Optimization). See class lago.h + * Output is written on a file, in g2o format + * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o + * @date May 15, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = "../../examples/Data/noisyToyGraph.txt"; + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = readG2o(g2oFile); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = *graph; + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graphWithPrior.print(); + + std::cout << "Computing LAGO estimate" << std::endl; + Values estimateLago = lago::initialize(graphWithPrior); + std::cout << "done!" << std::endl; + + if (argc < 3) { + estimateLago.print("estimateLago"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(*graph, estimateLago, outputFile); + std::cout << "done! " << std::endl; + } + + return 0; +} diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp new file mode 100644 index 000000000..904919d42 --- /dev/null +++ b/examples/SFMExample_SmartFactor.cpp @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample_SmartFactor.cpp + * @brief A structure-from-motion problem on a simulated dataset, using smart projection factor + * @author Duy-Nguyen Ta + * @author Jing Dong + */ + +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// For loading the data +#include "SFMdata.h" + +// 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, +// The calibration should be known. +#include + +// Also, we will initialize the robot at some location using a Prior factor. +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a +// trust-region method known as Powell's Degleg +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +#include + +using namespace std; +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef gtsam::SmartProjectionPoseFactor + SmartFactor; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + 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 + + // Create the set of ground-truth landmarks + 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) { + + // 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) { + + // generate the 2D measurement + SimpleCamera camera(poses[j], *K); + Point2 measurement = camera.project(points[i]); + + // call add() function to add measurment 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); + } + + // 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 + + // 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 + + graph.print("Factor Graph:\n"); + + // Create the data structure to hold the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + 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.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. + // 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. + Values landmark_result; + for (size_t i = 0; i < points.size(); ++i) { + + // 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. + 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); + } + + landmark_result.print("Landmark results:\n"); + + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp new file mode 100644 index 000000000..8a04d13a3 --- /dev/null +++ b/examples/StereoVOExample.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * 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 SteroVOExample.cpp + * @brief A stereo visual odometry example + * @date May 25, 2014 + * @author Stephen Camp + */ + +/** + * A 3D stereo visual odometry example + * - robot starts at origin + * -moves forward 1 meter + * -takes stereo readings on three landmarks + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv){ + //create graph object, add first pose at origin with key '1' + NonlinearFactorGraph graph; + Pose3 first_pose = Pose3(); + graph.push_back(NonlinearEquality(1, first_pose)); + + //create factor noise model with 3 sigmas of value 1 + const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); + //create stereo camera calibration object with .2m between cameras + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); + //create and add stereo factors between first pose (key value 1) and the three landmarks + graph.push_back(GenericStereoFactor(StereoPoint2(520, 480, 440), model, 1, 3, K)); + graph.push_back(GenericStereoFactor(StereoPoint2(120, 80, 440), model, 1, 4, K)); + graph.push_back(GenericStereoFactor(StereoPoint2(320, 280, 140), model, 1, 5, K)); + //create and add stereo factors between second pose and the three landmarks + graph.push_back(GenericStereoFactor(StereoPoint2(570, 520, 490), model, 2, 3, K)); + graph.push_back(GenericStereoFactor(StereoPoint2(70, 20, 490), model, 2, 4, K)); + graph.push_back(GenericStereoFactor(StereoPoint2(320, 270, 115), model, 2, 5, K)); + //create Values object to contain initial estimates of camera poses and landmark locations + Values initial_estimate = Values(); + //create and add iniital estimates + initial_estimate.insert(1, first_pose); + initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1))); + initial_estimate.insert(3, Point3(1, 1, 5)); + initial_estimate.insert(4, Point3(-1, 1, 5)); + initial_estimate.insert(5, Point3(0, -0.5, 5)); + //create Levenberg-Marquardt optimizer for resulting factor graph, optimize + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + Values result = optimizer.optimize(); + + result.print("Final result:\n"); + + return 0; +} \ No newline at end of file diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp new file mode 100644 index 000000000..b9ab663d9 --- /dev/null +++ b/examples/StereoVOExample_large.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + +* 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 SteroVOExample.cpp +* @brief A stereo visual odometry example +* @date May 25, 2014 +* @author Stephen Camp +*/ + + +/** + * A 3D stereo visual odometry example + * - robot starts at origin + * -moves forward, taking periodic stereo measurements + * -takes stereo readings of many landmarks + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv){ + + Values initial_estimate; + NonlinearFactorGraph graph; + const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); + + string calibration_loc = findExampleDataFile("VO_calibration.txt"); + string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); + string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); + + //read camera calibration info from file + // focal lengths fx, fy, skew s, principal point u0, v0, baseline b + double fx, fy, s, u0, v0, b; + ifstream calibration_file(calibration_loc.c_str()); + cout << "Reading calibration info" << endl; + calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; + + //create stereo camera calibration object + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); + + ifstream pose_file(pose_loc.c_str()); + cout << "Reading camera poses" << endl; + int pose_id; + MatrixRowMajor m(4,4); + //read camera pose parameters and use to make initial estimates of camera poses + while (pose_file >> pose_id) { + for (int i = 0; i < 16; i++) { + pose_file >> m.data()[i]; + } + initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + } + + // camera and landmark keys + size_t x, l; + + // pixel coordinates uL, uR, v (same for left/right images due to rectification) + // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + double uL, uR, v, X, Y, Z; + ifstream factor_file(factor_loc.c_str()); + cout << "Reading stereo factors" << endl; + //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation + while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + graph.push_back( + GenericStereoFactor(StereoPoint2(uL, uR, v), model, + Symbol('x', x), Symbol('l', l), K)); + //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it + if (!initial_estimate.exists(Symbol('l', l))) { + Pose3 camPose = initial_estimate.at(Symbol('x', x)); + //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space + Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); + initial_estimate.insert(Symbol('l', l), worldPoint); + } + } + + Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + //constrain the first pose such that it cannot change from its original value during optimization + // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky + // QR is much slower than Cholesky, but numerically more stable + graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + + cout << "Optimizing" << endl; + //create Levenberg-Marquardt optimizer to optimize the factor graph + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + Values result = optimizer.optimize(); + + cout << "Final result sample:" << endl; + Values pose_values = result.filter(); + pose_values.print("Final camera poses:\n"); + + return 0; +} diff --git a/gtsam.h b/gtsam.h index 2d181d350..b7178d534 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2249,6 +2249,13 @@ pair load2D(string filename, pair load2D(string filename); pair load2D_robust(string filename, gtsam::noiseModel::Base* model); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +pair readG2o(string filename); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); //************************************************************************* // Navigation @@ -2363,6 +2370,12 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { namespace utilities { #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); Matrix extractPoint2(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); @@ -2375,6 +2388,8 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); } //\namespace utilities diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 36ffa1ada..1c35ffa41 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -617,11 +617,12 @@ #include "ccolamd.h" -#include #include #include #ifdef MATLAB_MEX_FILE +#include +typedef uint16_t char16_t; #include "mex.h" #include "matrix.h" #endif diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c index d43985126..e470804a6 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c @@ -13,6 +13,9 @@ #ifndef NPRINT #ifdef MATLAB_MEX_FILE +#include +#include +typedef uint16_t char16_t; #include "mex.h" int (*ccolamd_printf) (const char *, ...) = mexPrintf ; #else diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 430ae9515..718179866 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -16,13 +16,30 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() endforeach(eigen_dir) + # do the same for the unsupported eigen folder + file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") + + file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*") + foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all}) + get_filename_component(filename ${unsupported_eigen_dir} NAME) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) + list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen) + endif() + endforeach(unsupported_eigen_dir) + # Add to project source set(eigen_headers ${eigen_headers} PARENT_SCOPE) + # set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE) # install Eigen - only the headers in our 3rdparty directory install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") + + install(DIRECTORY Eigen/unsupported/Eigen + DESTINATION include/gtsam/3rdparty/Eigen/unsupported/ + FILES_MATCHING PATTERN "*.h") endif() ############ NOTE: When updating GeographicLib be sure to disable building their examples diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d9ab7d71c..f51197cf2 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -591,41 +591,17 @@ Matrix3 skewSymmetric(double wx, double wy, double wz) } /* ************************************************************************* */ -/** Numerical Recipes in C wrappers - * create Numerical Recipes in C structure - * pointers are subtracted by one to provide base 1 access - */ -/* ************************************************************************* */ -// FIXME: assumes row major, rather than column major -//double** createNRC(Matrix& A) { -// const size_t m=A.rows(); -// double** a = new double* [m]; -// for(size_t i = 0; i < m; i++) -// a[i] = &A(i,0)-1; -// return a; -//} - -/* ****************************************** - * - * Modified from Justin's codebase - * - * Idea came from other public domain code. Takes a S.P.D. matrix - * and computes the LL^t decomposition. returns L, which is lower - * triangular. Note this is the opposite convention from Matlab, - * which calculates Q'Q where Q is upper triangular. - * - * ******************************************/ Matrix LLt(const Matrix& A) { - Matrix L = zeros(A.rows(), A.rows()); - Eigen::LLT llt; - llt.compute(A); + Eigen::LLT llt(A); return llt.matrixL(); } +/* ************************************************************************* */ Matrix RtR(const Matrix &A) { - return LLt(A).transpose(); + Eigen::LLT llt(A); + return llt.matrixU(); } /* @@ -633,13 +609,10 @@ Matrix RtR(const Matrix &A) */ Matrix cholesky_inverse(const Matrix &A) { - // FIXME: replace with real algorithm - return A.inverse(); - -// Matrix L = LLt(A); -// Matrix inv(eye(A.rows())); -// inplace_solve (L, inv, BNU::lower_tag ()); -// return BNU::prod(trans(inv), inv); + Eigen::LLT llt(A); + Matrix inv = eye(A.rows()); + llt.matrixU().solveInPlace(inv); + return inv*inv.transpose(); } /* ************************************************************************* */ @@ -648,9 +621,9 @@ Matrix cholesky_inverse(const Matrix &A) // inv(B) * inv(B)' == A // inv(B' * B) == A Matrix inverse_square_root(const Matrix& A) { - Matrix R = RtR(A); + Eigen::LLT llt(A); Matrix inv = eye(A.rows()); - R.triangularView().solveInPlace(inv); + llt.matrixU().solveInPlace(inv); return inv.transpose(); } diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 45edbf6af..5dcc79a68 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -236,6 +236,10 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + // Fill in the lower triangle part of the matrix, so boost::serialization won't + // complain about uninitialized data with an input_stream_error exception + // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error + matrix_.triangularView() = matrix_.triangularView().transpose(); ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); ar & BOOST_SERIALIZATION_NVP(blockStart_); diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 9d84e5ab7..4e857b143 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1022,22 +1022,32 @@ TEST( matrix, inverse_square_root ) /* *********************************************************************** */ // M was generated as the covariance of a set of random numbers. L that // we are checking against was generated via chol(M)' on octave +namespace cholesky { +Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, + 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, + 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, + 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, + -0.0021113, 0.0036415, 0.0909464); + +Matrix expected = (Matrix(5, 5) << + 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, + 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); +} TEST( matrix, LLt ) { - Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, - 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, - 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, - 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, - -0.0021113, 0.0036415, 0.0909464); + EQUALITY(cholesky::expected, LLt(cholesky::M)); +} +TEST( matrix, RtR ) +{ + EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); +} - Matrix expected = (Matrix(5, 5) << - 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, - 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); - - EQUALITY(expected, LLt(M)); +TEST( matrix, cholesky_inverse ) +{ + EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 30a4434fe..85307e322 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -178,6 +178,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional H1, // Calculate delta translation = unrotate(R1, dt); Point2 dt = p2.t() - t_; double x = dt.x(), y = dt.y(); + // t = R1' * (t2-t1) Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above diff --git a/gtsam_unstable/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h similarity index 100% rename from gtsam_unstable/geometry/TriangulationFactor.h rename to gtsam/geometry/TriangulationFactor.h diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 88359e0f8..db4c8da83 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -151,9 +151,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const { double dot = p.dot(q); // Check for special cases - if (std::abs(dot - 1.0) < 1e-20) + if (std::abs(dot - 1.0) < 1e-16) return (Vector(2) << 0, 0); - else if (std::abs(dot + 1.0) < 1e-20) + else if (std::abs(dot + 1.0) < 1e-16) return (Vector(2) << M_PI, 0); else { // no special case diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index a5646f647..42b548f5a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -398,7 +398,7 @@ TEST( Pose2, matrix ) TEST( Pose2, compose_matrix ) { Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y - Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x + Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) looking at negative x Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2)); EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT } @@ -412,7 +412,7 @@ TEST( Pose2, between ) // // *--0--*--* Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y - Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x + Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) looking at negative x Matrix actualH1,actualH2; Pose2 expected(M_PI/2.0, Point2(2,2)); diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp similarity index 99% rename from gtsam_unstable/geometry/tests/testTriangulation.cpp rename to gtsam/geometry/tests/testTriangulation.cpp index 8d45311f1..fb05bcf9f 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -16,7 +16,7 @@ * Author: cbeall3 */ -#include +#include #include #include diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testUnit3.cpp similarity index 100% rename from gtsam/geometry/tests/testSphere2.cpp rename to gtsam/geometry/tests/testUnit3.cpp diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp similarity index 98% rename from gtsam_unstable/geometry/triangulation.cpp rename to gtsam/geometry/triangulation.cpp index 3017fdf7a..9e1575801 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -16,7 +16,7 @@ * @author Chris Beall */ -#include +#include #include #include diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam/geometry/triangulation.h similarity index 97% rename from gtsam_unstable/geometry/triangulation.h rename to gtsam/geometry/triangulation.h index f767514c1..6899c616a 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,8 +18,8 @@ #pragma once -#include -#include + +#include #include #include #include @@ -52,7 +52,7 @@ public: * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT( +GTSAM_EXPORT Point3 triangulateDLT( const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); @@ -120,7 +120,7 @@ std::pair triangulationGraph( * @param landmarkKey to refer to landmark * @return refined Point3 */ -GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, +GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey); /** diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index a88ea5d57..82eb85c76 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -23,43 +23,35 @@ namespace gtsam { /* ************************************************************************* */ template -void VariableIndex::augment(const FG& factors, boost::optional&> newFactorIndices) -{ +void VariableIndex::augment(const FG& factors, + boost::optional&> newFactorIndices) { gttic(VariableIndex_augment); // Augment index for each factor - for(size_t i = 0; i < factors.size(); ++i) - { - if(factors[i]) - { + for (size_t i = 0; i < factors.size(); ++i) { + if (factors[i]) { const size_t globalI = - newFactorIndices ? - (*newFactorIndices)[i] : - nFactors_; - BOOST_FOREACH(const Key key, *factors[i]) - { + newFactorIndices ? (*newFactorIndices)[i] : nFactors_; + BOOST_FOREACH(const Key key, *factors[i]) { index_[key].push_back(globalI); - ++ nEntries_; + ++nEntries_; } } // Increment factor count even if factors are null, to keep indices consistent - if(newFactorIndices) - { - if((*newFactorIndices)[i] >= nFactors_) + if (newFactorIndices) { + if ((*newFactorIndices)[i] >= nFactors_) nFactors_ = (*newFactorIndices)[i] + 1; - } - else - { - ++ nFactors_; + } else { + ++nFactors_; } } } /* ************************************************************************* */ template -void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors) -{ +void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, + const FG& factors) { gttic(VariableIndex_remove); // NOTE: We intentionally do not decrement nFactors_ because the factor @@ -68,17 +60,20 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& // one greater than the highest-numbered factor referenced in a VariableIndex. ITERATOR factorIndex = firstFactor; size_t i = 0; - for( ; factorIndex != lastFactor; ++factorIndex, ++i) { - if(i >= factors.size()) - throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); - if(factors[i]) { + for (; factorIndex != lastFactor; ++factorIndex, ++i) { + if (i >= factors.size()) + throw std::invalid_argument( + "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); + if (factors[i]) { BOOST_FOREACH(Key j, *factors[i]) { Factors& factorEntries = internalAt(j); - Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); - if(entry == factorEntries.end()) - throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); + Factors::iterator entry = std::find(factorEntries.begin(), + factorEntries.end(), *factorIndex); + if (entry == factorEntries.end()) + throw std::invalid_argument( + "Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); factorEntries.erase(entry); - -- nEntries_; + --nEntries_; } } } @@ -87,10 +82,11 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& /* ************************************************************************* */ template void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { - for(ITERATOR key = firstKey; key != lastKey; ++key) { + for (ITERATOR key = firstKey; key != lastKey; ++key) { KeyMap::iterator entry = index_.find(*key); - if(!entry->second.empty()) - throw std::invalid_argument("Asking to remove variables from the variable index that are not unused"); + if (!entry->second.empty()) + throw std::invalid_argument( + "Asking to remove variables from the variable index that are not unused"); index_.erase(entry); } } diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 48ee88255..90c7b32c9 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -22,7 +22,7 @@ #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" -#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration" +//#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration" #endif #include #ifdef __GNUC__ @@ -73,34 +73,41 @@ SDGraph toBoostGraph(const G& graph) { SDGraph g; typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; std::map key2vertex; - BoostVertex v1, v2; typename G::const_iterator itFactor; + // Loop over the factors for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { - if ((*itFactor)->keys().size() > 2) - throw(std::invalid_argument("toBoostGraph: only support factors with at most two keys")); - if ((*itFactor)->keys().size() == 1) + // Ignore factors that are not binary + if ((*itFactor)->keys().size() != 2) continue; + // Cast the factor to the user-specified factor type F boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); + // Ignore factors that are not of type F if (!factor) continue; - KEY key1 = factor->key1(); - KEY key2 = factor->key2(); + // Retrieve the 2 keys (nodes) the factor (edge) is incident on + KEY key1 = factor->keys()[0]; + KEY key2 = factor->keys()[1]; + BoostVertex v1, v2; + + // If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id if (key2vertex.find(key1) == key2vertex.end()) { v1 = add_vertex(key1, g); - key2vertex.insert(make_pair(key1, v1)); + key2vertex.insert(std::pair(key1, v1)); } else v1 = key2vertex[key1]; + // If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id if (key2vertex.find(key2) == key2vertex.end()) { v2 = add_vertex(key2, g); - key2vertex.insert(make_pair(key2, v2)); + key2vertex.insert(std::pair(key2, v2)); } else v2 = key2vertex[key2]; + // Add an edge with weight 1.0 boost::property edge_property(1.0); // assume constant edge weight here boost::add_edge(v1, v2, edge_property, g); } @@ -222,12 +229,11 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap return config; } -/* ************************************************************************* */ - /* ************************************************************************* */ template PredecessorMap findMinimumSpanningTree(const G& fg) { + // Convert to a graph that boost understands SDGraph g = gtsam::toBoostGraph(fg); // find minimum spanning tree @@ -237,13 +243,12 @@ PredecessorMap findMinimumSpanningTree(const G& fg) { // convert edge to string pairs PredecessorMap tree; typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; - typename std::vector::Vertex>::iterator vi; - for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) { + BOOST_FOREACH(const typename SDGraph::Vertex& vi, p_map){ KEY key = boost::get(boost::vertex_name, g, *itVertex); - KEY parent = boost::get(boost::vertex_name, g, *vi); + KEY parent = boost::get(boost::vertex_name, g, vi); tree.insert(key, parent); + itVertex++; } - return tree; } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 543822ce0..8c8e2cb2b 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -130,6 +130,9 @@ namespace gtsam { /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; + /// A'*b for Jacobian, eta for Hessian (raw memory version) + virtual void gradientAtZero(double* d) const = 0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b56270a55..d6836783b 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -70,11 +70,18 @@ namespace gtsam { vector dims_accumulated; dims_accumulated.resize(dims.size()+1,0); dims_accumulated[0]=0; - for (int i=1; i DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + DVector dj = -info_(pos,size()).knownOffDiagonal(); + DMap(d + 9 * j) += dj; + } +} + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index faf2ccd33..62d84925c 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -387,6 +387,8 @@ namespace gtsam { /// eta for Hessian VectorValues gradientAtZero() const; + virtual void gradientAtZero(double* d) const; + /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c080f75cb..a63bbf473 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -573,6 +573,11 @@ VectorValues JacobianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +void JacobianFactor::gradientAtZero(double* d) const { + //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); +} + /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 5a567c2c7..b90012822 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -286,6 +286,9 @@ namespace gtsam { /// A'*b for Jacobian VectorValues gradientAtZero() const; + /* ************************************************************************* */ + virtual void gradientAtZero(double* d) const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 641b47640..fcaec3afa 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -47,22 +47,73 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); } +/* ************************************************************************* */ +// check *above the diagonal* for non-zero entries +boost::optional checkIfDiagonal(const Matrix M) { + size_t m = M.rows(), n = M.cols(); + // check all non-diagonal entries + bool full = false; + size_t i, j; + for (i = 0; i < m; i++) + if (!full) + for (j = i + 1; j < n; j++) + if (fabs(M(i, j)) > 1e-9) { + full = true; + break; + } + if (full) { + return boost::none; + } else { + Vector diagonal(n); + for (j = 0; j < n; j++) + diagonal(j) = M(j, j); + return diagonal; + } +} /* ************************************************************************* */ -Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { - size_t m = covariance.rows(), n = covariance.cols(); - if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); - if (smart) { - // check all non-diagonal entries - size_t i,j; - for (i = 0; i < m; i++) - for (j = 0; j < n; j++) - if (i != j && fabs(covariance(i, j)) > 1e-9) goto full; - Vector variances(n); - for (j = 0; j < n; j++) variances(j) = covariance(j,j); - return Diagonal::Variances(variances,true); +Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { + size_t m = R.rows(), n = R.cols(); + if (m != n) + throw invalid_argument("Gaussian::SqrtInformation: R not square"); + boost::optional diagonal = boost::none; + if (smart) + diagonal = checkIfDiagonal(R); + if (diagonal) + return Diagonal::Sigmas(reciprocal(*diagonal), true); + else + return shared_ptr(new Gaussian(R.rows(), R)); +} + +/* ************************************************************************* */ +Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { + size_t m = M.rows(), n = M.cols(); + if (m != n) + throw invalid_argument("Gaussian::Information: R not square"); + boost::optional diagonal = boost::none; + if (smart) + diagonal = checkIfDiagonal(M); + if (diagonal) + return Diagonal::Precisions(*diagonal, true); + else { + Matrix R = RtR(M); + return shared_ptr(new Gaussian(R.rows(), R)); } - full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); +} + +/* ************************************************************************* */ +Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, + bool smart) { + size_t m = covariance.rows(), n = covariance.cols(); + if (m != n) + throw invalid_argument("Gaussian::Covariance: covariance not square"); + boost::optional variances = boost::none; + if (smart) + variances = checkIfDiagonal(covariance); + if (variances) + return Diagonal::Variances(*variances, true); + else + return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); } /* ************************************************************************* */ @@ -166,7 +217,7 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const // Diagonal /* ************************************************************************* */ Diagonal::Diagonal() : - Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) + Gaussian(1) // TODO: Frank asks: really sure about this? { } @@ -180,8 +231,8 @@ Diagonal::Diagonal(const Vector& sigmas) : Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { if (smart) { // check whether all the same entry - DenseIndex j, n = variances.size(); - for (j = 1; j < n; j++) + size_t n = variances.size(); + for (size_t j = 1; j < n; j++) if (variances(j) != variances(0)) goto full; return Isotropic::Variance(n, variances(0), true); } @@ -191,12 +242,18 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { /* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { if (smart) { + size_t n = sigmas.size(); + if (n==0) goto full; // look for zeros to make a constraint - for (size_t i=0; i< (size_t) sigmas.size(); ++i) - if (sigmas(i)<1e-8) + for (size_t j=0; j< n; ++j) + if (sigmas(j)<1e-8) return Constrained::MixedSigmas(sigmas); + // check whether all the same entry + for (size_t j = 1; j < n; j++) + if (sigmas(j) != sigmas(0)) goto full; + return Isotropic::Sigma(n, sigmas(0), true); } - return Diagonal::shared_ptr(new Diagonal(sigmas)); + full: return Diagonal::shared_ptr(new Diagonal(sigmas)); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index a87f426fa..0351cfabd 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -159,10 +159,17 @@ namespace gtsam { /** * A Gaussian noise model created by specifying a square root information matrix. + * @param R The (upper-triangular) square root information matrix + * @param smart check if can be simplified to derived class */ - static shared_ptr SqrtInformation(const Matrix& R) { - return shared_ptr(new Gaussian(R.rows(),R)); - } + static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); + + /** + * A Gaussian noise model created by specifying an information matrix. + * @param M The information matrix + * @param smart check if can be simplified to derived class + */ + static shared_ptr Information(const Matrix& M, bool smart = true); /** * A Gaussian noise model created by specifying a covariance matrix. @@ -864,6 +871,9 @@ namespace gtsam { ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); } }; + + // Helper function + GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix M); } // namespace noiseModel diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 9d87a163b..df0f8a774 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -267,6 +267,35 @@ TEST(NoiseModel, QRNan ) EXPECT(assert_equal(expectedAb,Ab)); } +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Create(3); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation2 ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +TEST(NoiseModel, SmartInformation ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2); + Matrix M = 0.5*eye(3); + EXPECT(checkIfDiagonal(M)); + gtsam::SharedGaussian actual = Gaussian::Information(M, smart); + EXPECT(assert_equal(*expected,*actual)); +} + /* ************************************************************************* */ TEST(NoiseModel, SmartCovariance ) { diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 559568c84..06e79324d 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -34,6 +34,11 @@ namespace gtsam { * * @addtogroup SLAM * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * * REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b1e20297e..f5b5293f8 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -33,7 +33,13 @@ namespace gtsam { /** * * @addtogroup SLAM - * * REFERENCES: + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + ** REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 79677c0c6..44f543bc9 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -54,8 +54,11 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); - if (HR) - *HR = HR->col(2); + if (HR) { + // assign to temporary first to avoid error in Win-Debug mode + Matrix H = HR->col(2); + *HR = H; + } return q; } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index a6c04681b..dea4113f7 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) stay = false; // If not searching, just return with the new Delta else if(mode == SEARCH_EACH_ITERATION) { - if(newDelta == Delta || lastAction == DECREASED_DELTA) + if(fabs(newDelta - Delta) < 1e-15 || lastAction == DECREASED_DELTA) stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region else { stay = true; // Searching and increased Delta, so try again to increase Delta diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 6be69e710..08961db86 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -138,7 +138,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { } /* ************************************************************************* */ -GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( +GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( const GaussianFactorGraph& linear) { gttic(damp); @@ -159,7 +159,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( // for each of the variables, add a prior double sigma = 1.0 / std::sqrt(state_.lambda); - GaussianFactorGraph damped = linear; + GaussianFactorGraph::shared_ptr dampedPtr = linear.cloneToPtr(); + GaussianFactorGraph &damped = (*dampedPtr); damped.reserve(damped.size() + state_.values.size()); if (params_.diagonalDamping) { BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) { @@ -188,7 +189,20 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( } } gttoc(damp); - return damped; + return dampedPtr; +} + +/* ************************************************************************* */ +// Log current error/lambda to file +inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){ + if (!params_.logFile.empty()) { + ofstream os(params_.logFile.c_str(), ios::app); + boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); + os << /*inner iterations*/ state_.totalNumberInnerIterations << "," + << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," + << /*current error*/ currentError << "," << state_.lambda << "," + << /*outer iterations*/ state_.iterations << endl; + } } /* ************************************************************************* */ @@ -205,6 +219,9 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); + if(state_.totalNumberInnerIterations==0) // write initial error + writeLogFile(state_.error); + // Keep increasing lambda until we make make progress while (true) { @@ -212,21 +229,8 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "trying lambda = " << state_.lambda << endl; // Build damped system for this lambda (adds prior factors that make it like gradient descent) - GaussianFactorGraph dampedSystem = buildDampedSystem(*linear); - - // Log current error/lambda to file - if (!params_.logFile.empty()) { - ofstream os(params_.logFile.c_str(), ios::app); - - boost::posix_time::ptime currentTime = - boost::posix_time::microsec_clock::universal_time(); - - os << state_.totalNumberInnerIterations << "," - << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," - << state_.error << "," << state_.lambda << endl; - } - - ++state_.totalNumberInnerIterations; + GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear); + GaussianFactorGraph &dampedSystem = (*dampedSystemPtr); // Try solving double modelFidelity = 0.0; @@ -240,7 +244,7 @@ void LevenbergMarquardtOptimizer::iterate() { try { delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch (IndeterminantLinearSystemException& e) { + } catch (IndeterminantLinearSystemException) { systemSolvedSuccessfully = false; } @@ -256,6 +260,9 @@ void LevenbergMarquardtOptimizer::iterate() { double newlinearizedError = linear->error(delta); double linearizedCostChange = state_.error - newlinearizedError; + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "newlinearizedError = " << newlinearizedError << + " linearizedCostChange = " << linearizedCostChange << endl; if (linearizedCostChange >= 0) { // step is valid // update values @@ -266,50 +273,62 @@ void LevenbergMarquardtOptimizer::iterate() { // compute new error gttic(compute_error); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "calculating error" << endl; + cout << "calculating error:" << endl; newError = graph_.error(newValues); gttoc(compute_error); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "old error (" << state_.error + << ") new (tentative) error (" << newError << ")" << endl; + // cost change in the original, nonlinear system (old - new) double costChange = state_.error - newError; - if (linearizedCostChange > 1e-15) { // the error has to decrease to satify this condition + if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition // fidelity of linearized model VS original system between modelFidelity = costChange / linearizedCostChange; // if we decrease the error in the nonlinear system and modelFidelity is above threshold step_is_successful = modelFidelity > params_.minModelFidelity; - } else { - step_is_successful = true; // linearizedCostChange close to zero - } + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "modelFidelity: " << modelFidelity << endl; + } // else we consider the step non successful and we either increase lambda or stop if error change is small double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; // if the change is small we terminate - if (fabs(costChange) < minAbsoluteTolerance) + if (fabs(costChange) < minAbsoluteTolerance){ + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "fabs(costChange)="<= LevenbergMarquardtParams::TRYLAMBDA) - cout << "increasing lambda: old error (" << state_.error - << ") new error (" << newError << ")" << endl; + cout << "increasing lambda" << endl; increaseLambda(); + writeLogFile(state_.error); // check if lambda is too big if (state_.lambda >= params_.lambdaUpperBound) { if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION) - cout - << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" - << endl; + cout << "Warning: Levenberg-Marquardt giving up because " + "cannot decrease error with maximum lambda" << endl; break; } } else { // the change in the cost is very small and it is not worth trying bigger lambdas + writeLogFile(state_.error); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl; break; } } // end while diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 47952f9e4..f365fc524 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -113,7 +113,6 @@ public: inline void setDiagonalDamping(bool flag) { diagonalDamping = flag; } - inline void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor_ = flag; } @@ -255,9 +254,11 @@ public: } /** Build a damped system for a specific lambda */ - GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear); + GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear); friend class ::NonlinearOptimizerMoreOptimizationTest; + void writeLogFile(double currentError); + /// @} protected: diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam/slam/ImplicitSchurFactor.h similarity index 75% rename from gtsam_unstable/slam/ImplicitSchurFactor.h rename to gtsam/slam/ImplicitSchurFactor.h index 234d13f1f..c0f339b30 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam/slam/ImplicitSchurFactor.h @@ -24,7 +24,6 @@ class ImplicitSchurFactor: public GaussianFactor { public: typedef ImplicitSchurFactor This; ///< Typedef to this class - typedef JacobianFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class protected: @@ -87,7 +86,8 @@ public: } /// print - void print(const std::string& s, const KeyFormatter& formatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << " ImplicitSchurFactor " << std::endl; Factor::print(s); std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; @@ -188,19 +188,24 @@ public: /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const { std::map blocks; + // F'*(I - E*P*E')*F for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; - const Matrix2D& Fj = Fblocks_[pos].second; // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) - Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); - blocks[j] = Fj.transpose() * Fj - - FtE * PointCovariance_ * FtE.transpose(); + const Matrix2D& Fj = Fblocks_[pos].second; + // Eigen::Matrix FtE = Fj.transpose() + // * E_.block<2, 3>(2 * pos, 0); + // blocks[j] = Fj.transpose() * Fj + // - FtE * PointCovariance_ * FtE.transpose(); + + const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0); + blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); + // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( -// static const Eigen::Matrix I2 = eye(2); -// Eigen::Matrix Q = // -// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); -// blocks[j] = Fj.transpose() * Q * Fj; + // static const Eigen::Matrix I2 = eye(2); + // Eigen::Matrix Q = // + // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); + // blocks[j] = Fj.transpose() * Q * Fj; } return blocks; } @@ -235,25 +240,33 @@ public: typedef std::vector Error2s; /** - * @brief Calculate corrected error Q*e = (I - E*P*E')*e + * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b) */ - void projectError(const Error2s& e1, Error2s& e2) const { + void projectError2(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * e1 = (3*2m)*2m + // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2)); // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; + e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2; } - /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + /* + * This definition matches the linearized error in the Hessian Factor: + * LinError(x) = x'*H*x - 2*x'*eta + f + * with: + * H = F' * (I-E'*P*E) * F = F' * Q * F + * eta = F' * (I-E'*P*E) * b = F' * Q * b + * f = nonlinear error + * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f + */ virtual double error(const VectorValues& x) const { // resize does not do malloc if correct size @@ -262,15 +275,56 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); - projectError(e1, e2); + e1[k] = Fblocks_[k].second * x.at(keys_[k]); + projectError2(e1, e2); double result = 0; for (size_t k = 0; k < size(); ++k) - result += dot(e2[k], e2[k]); - return 0.5 * result; + result += dot(e1[k], e2[k]); + + double f = b_.squaredNorm(); + return 0.5 * (result + f); } + /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + // This is wrong and does not match the definition in Hessian + // virtual double error(const VectorValues& x) const { + // + // // resize does not do malloc if correct size + // e1.resize(size()); + // e2.resize(size()); + // + // // e1 = F * x - b = (2m*dm)*dm + // for (size_t k = 0; k < size(); ++k) + // e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + // projectError(e1, e2); + // + // double result = 0; + // for (size_t k = 0; k < size(); ++k) + // result += dot(e2[k], e2[k]); + // + // std::cout << "implicitFactor::error result " << result << std::endl; + // return 0.5 * result; + // } + /** + * @brief Calculate corrected error Q*e = (I - E*P*E')*e + */ + void projectError(const Error2s& e1, Error2s& e2) const { + + // d1 = E.transpose() * e1 = (3*2m)*2m + Vector3 d1; + d1.setZero(); + for (size_t k = 0; k < size(); k++) + d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + + // d2 = E.transpose() * e1 = (3*2m)*2m + Vector3 d2 = PointCovariance_ * d1; + + // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] + for (size_t k = 0; k < size(); k++) + e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; + } + /// Scratch space for multiplyHessianAdd mutable Error2s e1, e2; @@ -377,6 +431,28 @@ public: return g; } + /** + * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS + */ + void gradientAtZero(double* d) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // calculate Q*b + e1.resize(size()); + e2.resize(size()); + for (size_t k = 0; k < size(); k++) + e1[k] = b_.segment < 2 > (2 * k); + projectError(e1, e2); + + for (size_t k = 0; k < size(); ++k) { // for each camera in the factor + Key j = keys_[k]; + DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; + } + } + }; // ImplicitSchurFactor diff --git a/gtsam_unstable/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h similarity index 71% rename from gtsam_unstable/slam/JacobianFactorQ.h rename to gtsam/slam/JacobianFactorQ.h index fdbe0e231..f4dfb9422 100644 --- a/gtsam_unstable/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -23,6 +23,19 @@ public: JacobianFactorQ() { } + /// Empty constructor with keys + JacobianFactorQ(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + typedef std::pair KeyMatrix; + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, diff --git a/gtsam_unstable/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h similarity index 96% rename from gtsam_unstable/slam/JacobianFactorQR.h rename to gtsam/slam/JacobianFactorQR.h index 2d2d5b7a4..a928106a8 100644 --- a/gtsam_unstable/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,7 +6,7 @@ */ #pragma once -#include +#include namespace gtsam { /** diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h similarity index 69% rename from gtsam_unstable/slam/JacobianFactorSVD.h rename to gtsam/slam/JacobianFactorSVD.h index 752a9f48e..e28185038 100644 --- a/gtsam_unstable/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,7 +5,7 @@ */ #pragma once -#include "gtsam_unstable/slam/JacobianSchurFactor.h" +#include "gtsam/slam/JacobianSchurFactor.h" namespace gtsam { /** @@ -18,10 +18,23 @@ public: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; + typedef std::pair KeyMatrix; /// Default constructor JacobianFactorSVD() {} + /// Empty constructor with keys + JacobianFactorSVD(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { @@ -32,7 +45,6 @@ public: // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) diff --git a/gtsam_unstable/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h similarity index 100% rename from gtsam_unstable/slam/JacobianSchurFactor.h rename to gtsam/slam/JacobianSchurFactor.h diff --git a/gtsam_unstable/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h similarity index 100% rename from gtsam_unstable/slam/RegularHessianFactor.h rename to gtsam/slam/RegularHessianFactor.h diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h similarity index 79% rename from gtsam_unstable/slam/SmartFactorBase.h rename to gtsam/slam/SmartFactorBase.h index d24a878bb..1235fc6fb 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -20,6 +20,7 @@ #pragma once #include "JacobianFactorQ.h" +#include "JacobianFactorSVD.h" #include "ImplicitSchurFactor.h" #include "RegularHessianFactor.h" @@ -135,12 +136,12 @@ public: } /** return the measurements */ - const Vector& measured() const { + const std::vector& measured() const { return measured_; } /** return the noise model */ - const SharedNoiseModel& noise() const { + const std::vector& noise() const { return noise_; } @@ -192,7 +193,7 @@ public: b[2 * i + 1] = e.y(); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } i += 1; } @@ -222,7 +223,7 @@ public: * this->noise_.at(i)->distance(reprojectionError.vector()); } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } i += 1; } @@ -244,7 +245,7 @@ public: cameras[i].project(point, boost::none, Ei); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } this->noise_.at(i)->WhitenSystem(Ei, b); E.block<2, 3>(2 * i, 0) = Ei; @@ -274,7 +275,7 @@ public: -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); @@ -302,15 +303,18 @@ public: // Point covariance inv(E'*E) Matrix3 EtE = E.transpose() * E; - Matrix3 DMatrix = eye(E.cols()); // damping matrix if (diagonalDamping) { // diagonal of the hessian - DMatrix(0, 0) = EtE(0, 0); - DMatrix(1, 1) = EtE(1, 1); - DMatrix(2, 2) = EtE(2, 2); + EtE(0, 0) += lambda * EtE(0, 0); + EtE(1, 1) += lambda * EtE(1, 1); + EtE(2, 2) += lambda * EtE(2, 2); + }else{ + EtE(0, 0) += lambda; + EtE(1, 1) += lambda; + EtE(2, 2) += lambda; } - PointCov.noalias() = (EtE + lambda * DMatrix).inverse(); + PointCov.noalias() = (EtE).inverse(); return f; } @@ -322,7 +326,7 @@ public: const double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(2 * numKeys, D * numKeys); @@ -345,10 +349,10 @@ public: diagonalDamping); // diagonalDamping should have no effect (only on PointCov) // Do SVD on A - Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU); + Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); // Enull = zeros(2 * numKeys, 2 * numKeys - 3); - int numKeys = this->keys_.size(); + size_t numKeys = this->keys_.size(); Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns return f; @@ -361,7 +365,7 @@ public: const Cameras& cameras, const Point3& point) const { int numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); F.resize(2 * numKeys, D * numKeys); F.setZero(); @@ -380,14 +384,14 @@ public: int numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; Matrix E; Matrix3 PointCov; Vector b; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); -//#define HESSIAN_BLOCKS +//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix #ifdef HESSIAN_BLOCKS // Create structures for Hessian Factors std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); @@ -400,46 +404,23 @@ public: //std::vector < Vector > gs2(gs.begin(), gs.end()); return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, f); -#else + > (this->keys_, Gs, gs, f); +#else // we create directly a SymmetricBlockMatrix size_t n1 = D * numKeys + 1; std::vector dims(numKeys + 1); // this also includes the b term std::fill(dims.begin(), dims.end() - 1, D); dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - augmentedHessian(numKeys,numKeys)(0,0) = f; - return boost::make_shared >( - this->keys_, augmentedHessian); - + augmentedHessian(numKeys, numKeys)(0, 0) = f; + return boost::make_shared >(this->keys_, + augmentedHessian); #endif } // **************************************************************************************************** - boost::shared_ptr > updateAugmentedHessian( - const Cameras& cameras, const Point3& point, const double lambda, - bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const { - - int numKeys = this->keys_.size(); - - std::vector < KeyMatrix2D > Fblocks; - Matrix E; - Matrix3 PointCov; - Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - std::vector dims(numKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, D); - dims.back() = 1; - - updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - std::cout << "f "<< f <& Fblocks, const Matrix& E, const Matrix& PointCov, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { @@ -466,7 +447,7 @@ public: int GsCount2 = 0; for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); + gs.at(i1) = gs_vector.segment(i1D); for (DenseIndex i2 = 0; i2 < numKeys; i2++) { if (i2 >= i1) { Gs.at(GsCount2) = H.block(i1D, i2 * D); @@ -476,53 +457,6 @@ public: } } - // **************************************************************************************************** - void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); // cameras observing current point - size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; - - // D = (Dx2) * (2) - // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - size_t aug_i1 = this->keys_[i1]; - std::cout << "i1 "<< i1 < (2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - std::cout << "filled 1 " <(2 * i1, 0).transpose() * Fi1); - - // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - size_t aug_i2 = this->keys_[i2]; - std::cout << "i2 "<< i2 <(2 * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - } - // **************************************************************************************************** void sparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, @@ -542,20 +476,20 @@ public: // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - augmentedHessian(i1,i1) = - Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + augmentedHessian(i1, i1) = Fi1.transpose() + * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i1,i2) = - -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + augmentedHessian(i1, i2) = -Fi1.transpose() + * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); } } // end of for over cameras } @@ -586,24 +520,109 @@ public: { // for i1 = i2 // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b)); + gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + Gs.at(GsIndex) = Fi1.transpose() + * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); GsIndex++; } // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + Gs.at(GsIndex) = -Fi1.transpose() + * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); GsIndex++; } } // end of for over cameras } + + // **************************************************************************************************** + void updateAugmentedHessian(const Cameras& cameras, const Point3& point, + const double lambda, bool diagonalDamping, + SymmetricBlockMatrix& augmentedHessian, + const FastVector allKeys) const { + + // int numKeys = this->keys_.size(); + + std::vector Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + + updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + } + + // **************************************************************************************************** + void updateSparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const double f, const FastVector allKeys, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { + // Schur complement trick + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) + + MatrixDD matrixBlock; + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + + FastMap KeySlotMap; + for (size_t slot=0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); // cameras observing current point + size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group + + // Blockwise Schur complement + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor + + const Matrix2D& Fi1 = Fblocks.at(i1).second; + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + // D = (Dx2) * (2) + // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) + // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) + // Key cameraKey_i1 = this->keys_[i1]; + DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]]; + + // information vector - store previous vector + // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() + + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // main block diagonal - store previous block + matrixBlock = augmentedHessian(aug_i1, aug_i1); + // add contribution of current factor + augmentedHessian(aug_i1, aug_i1) = matrixBlock + + ( Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1) ); + + // upper triangular part of the hessian + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera + const Matrix2D& Fi2 = Fblocks.at(i2).second; + + //Key cameraKey_i2 = this->keys_[i2]; + DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + // off diagonal block - store previous block + // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() + - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + } + } // end of for over cameras + + augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; + } + // **************************************************************************************************** boost::shared_ptr > createImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, @@ -620,13 +639,24 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; Matrix E; Matrix3 PointCov; Vector b; computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); - return boost::make_shared < JacobianFactorQ > (Fblocks, E, PointCov, b); + return boost::make_shared >(Fblocks, E, PointCov, b); + } + + // **************************************************************************************************** + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0) const { + size_t numKeys = this->keys_.size(); + std::vector < KeyMatrix2D > Fblocks; + Vector b; + Matrix Enull(2*numKeys, 2*numKeys-3); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); + return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); } private: diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h similarity index 91% rename from gtsam_unstable/slam/SmartProjectionFactor.h rename to gtsam/slam/SmartProjectionFactor.h index 59a841813..043528fea 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -21,11 +21,10 @@ #include "SmartFactorBase.h" -#include +#include #include #include #include -#include #include #include @@ -54,6 +53,10 @@ public: double f; }; +enum LinearizationMode { + HESSIAN, JACOBIAN_SVD, JACOBIAN_Q +}; + /** * SmartProjectionFactor: triangulates point * TODO: why LANDMARK parameter? @@ -91,6 +94,13 @@ protected: /// shorthand for base class type typedef SmartFactorBase Base; + double landmarkDistanceThreshold_; // if the landmark is triangulated at a + // distance larger than that the factor is considered degenerate + + double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the + // average reprojection error is smaller than this threshold after triangulation, + // and the factor is disregarded if the error is large + /// shorthand for this class typedef SmartProjectionFactor This; @@ -115,12 +125,15 @@ public: SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr( - new SmartProjectionFactorState())) : + double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1, + SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state) { + false), verboseCheirality_(false), state_(state), + landmarkDistanceThreshold_(landmarkDistanceThreshold), + dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -234,6 +247,31 @@ public: rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i=0; + BOOST_FOREACH(const Camera& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + degenerate_ = true; + break; + } + const Point2& zi = this->measured_.at(i); + try { + Point2 reprojectionError(camera.project(point_) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException) { + cheiralityException_ = true; + } + i += 1; + } + // we discard smart factors that have large reprojection error + if(dynamicOutlierRejectionThreshold_ > 0 && + totalReprojError/m > dynamicOutlierRejectionThreshold_) + degenerate_ = true; + } catch (TriangulationUnderconstrainedException&) { // if TriangulationUnderconstrainedException can be // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before @@ -385,7 +423,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values @@ -397,7 +435,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); + } + + /// different (faster) way to compute Jacobian factor + boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianSVDFactor(cameras, point_, lambda); + else + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h similarity index 69% rename from gtsam_unstable/slam/SmartProjectionPoseFactor.h rename to gtsam/slam/SmartProjectionPoseFactor.h index 879e5ab80..273102bda 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -22,6 +22,16 @@ #include "SmartProjectionFactor.h" namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + */ /** * The calibration is known here. The factor only constraints poses (variable dimension is 6) @@ -31,7 +41,8 @@ template class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: - // Known calibration + LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) + std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) public: @@ -56,8 +67,11 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} @@ -65,7 +79,7 @@ public: /** * add a new measurement and pose key * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKey is the index corresponding to the camera observing the same landmark + * @param poseKey is key corresponding to the camera observing the same landmark * @param noise_i is the measurement noise * @param K_i is the (known) camera calibration */ @@ -77,8 +91,11 @@ public: } /** - * add a new measurements and pose keys - * Variant of the previous one in which we include a set of measurements + * Variant of the previous one in which we include a set of measurements + * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKeys vector of keys corresponding to the camera observing the same landmark + * @param noises vector of measurement noises + * @param Ks vector of calibration objects */ void add(std::vector measurements, std::vector poseKeys, std::vector noises, @@ -90,8 +107,11 @@ public: } /** - * add a new measurements and pose keys * Variant of the previous one in which we include a set of measurements with the same noise and calibration + * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKeys vector of keys corresponding to the camera observing the same landmark + * @param noise measurement noise (same for all measurements) + * @param K the (known) camera calibration (same for all measurements) */ void add(std::vector measurements, std::vector poseKeys, const SharedNoiseModel noise, const boost::shared_ptr K) { @@ -126,7 +146,12 @@ public: return 6 * this->keys_.size(); } - // Collect all cameras + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses corresponding + * to keys involved in this factor + * @return vector of Values + */ typename Base::Cameras cameras(const Values& values) const { typename Base::Cameras cameras; size_t i=0; @@ -139,11 +164,24 @@ public: } /** - * linearize returns a Hessian factor contraining the poses + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return */ virtual boost::shared_ptr linearize( const Values& values) const { - return this->createHessianFactor(cameras(values)); + // depending on flag set on construction we may linearize to different linear factors + switch(linearizeTo_){ + case JACOBIAN_SVD : + return this->createJacobianSVDFactor(cameras(values), 0.0); + break; + case JACOBIAN_Q : + return this->createJacobianQFactor(cameras(values), 0.0); + break; + default: + return this->createHessianFactor(cameras(values)); + break; + } } /** @@ -158,7 +196,7 @@ public: } /** return the calibration object */ - inline const boost::shared_ptr calibration() const { + inline const std::vector > calibration() const { return K_all_; } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4a7166f38..b3c9b9557 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -16,18 +16,18 @@ * @brief utility functions for loading datasets */ -#include -#include -#include - -#include - -#include -#include -#include #include #include #include +#include +#include +#include + +#include + +#include +#include +#include using namespace std; namespace fs = boost::filesystem; @@ -43,7 +43,7 @@ string findExampleDataFile(const string& name) { // Search source tree and installed location vector rootsToSearch; rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt - rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt + rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt // Search for filename as given, and with .graph and .txt extensions vector namesToSearch; @@ -55,35 +55,122 @@ string findExampleDataFile(const string& name) { // Find first name that exists BOOST_FOREACH(const fs::path& root, rootsToSearch) { BOOST_FOREACH(const fs::path& name, namesToSearch) { - if(fs::is_regular_file(root / name)) + if (fs::is_regular_file(root / name)) return (root / name).string(); } } // If we did not return already, then we did not find the file - throw std::invalid_argument( - "gtsam::findExampleDataFile could not find a matching file in\n" - SOURCE_TREE_DATASET_DIR " or\n" - INSTALLED_DATASET_DIR " named\n" + - name + ", " + name + ".graph, or " + name + ".txt"); + throw +invalid_argument( + "gtsam::findExampleDataFile could not find a matching file in\n" + SOURCE_TREE_DATASET_DIR " or\n" + INSTALLED_DATASET_DIR " named\n" + + name + ", " + name + ".graph, or " + name + ".txt"); } + +/* ************************************************************************* */ +string createRewrittenFileName(const string& name) { + // Search source tree and installed location + if (!exists(fs::path(name))) { + throw invalid_argument( + "gtsam::createRewrittenFileName could not find a matching file in\n" + + name); + } + + fs::path p(name); + fs::path newpath = fs::path(p.parent_path().string()) + / fs::path(p.stem().string() + "-rewritten.txt"); + + return newpath.string(); +} +/* ************************************************************************* */ + #endif /* ************************************************************************* */ -pair load2D( - pair > dataset, - int maxID, bool addNoise, bool smart) { - return load2D(dataset.first, dataset.second, maxID, addNoise, smart); +GraphAndValues load2D(pair dataset, int maxID, + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + return load2D(dataset.first, dataset.second, maxID, addNoise, smart, + noiseFormat, kernelFunctionType); } /* ************************************************************************* */ -pair load2D( - const string& filename, boost::optional model, int maxID, - bool addNoise, bool smart) { - cout << "Will try to read " << filename << endl; +// Read noise parameters and interpret them according to flags +static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, + NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { + double v1, v2, v3, v4, v5, v6; + is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; + + // Read matrix and check that diagonal entries are non-zero + Matrix M(3, 3); + switch (noiseFormat) { + case NoiseFormatG2O: + case NoiseFormatCOV: + // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] + if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0) + throw runtime_error( + "load2D::readNoiseModel looks like this is not G2O matrix order"); + M << v1, v2, v3, v2, v4, v5, v3, v5, v6; + break; + case NoiseFormatTORO: + case NoiseFormatGRAPH: + // http://www.openslam.org/toro.html + // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr + // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] + if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0) + throw invalid_argument( + "load2D::readNoiseModel looks like this is not TORO matrix order"); + M << v1, v2, v5, v2, v3, v6, v5, v6, v4; + break; + default: + throw runtime_error("load2D: invalid noise format"); + } + + // Now, create a Gaussian noise model + // The smart flag will try to detect a simpler model, e.g., unit + SharedNoiseModel model; + switch (noiseFormat) { + case NoiseFormatG2O: + case NoiseFormatTORO: + // In both cases, what is stored in file is the information matrix + model = noiseModel::Gaussian::Information(M, smart); + break; + case NoiseFormatGRAPH: + case NoiseFormatCOV: + // These cases expect covariance matrix + model = noiseModel::Gaussian::Covariance(M, smart); + break; + default: + throw invalid_argument("load2D: invalid noise format"); + } + + switch (kernelFunctionType) { + case KernelFunctionTypeNONE: + return model; + break; + case KernelFunctionTypeHUBER: + return noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), model); + break; + case KernelFunctionTypeTUKEY: + return noiseModel::Robust::Create( + noiseModel::mEstimator::Tukey::Create(4.6851), model); + break; + default: + throw invalid_argument("load2D: invalid kernel function type"); + } +} + +/* ************************************************************************* */ +GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, + bool addNoise, bool smart, NoiseFormat noiseFormat, + KernelFunctionType kernelFunctionType) { + ifstream is(filename.c_str()); if (!is) - throw std::invalid_argument("load2D: can not find the file!"); + throw invalid_argument("load2D: can not find file " + filename); Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); @@ -92,16 +179,18 @@ pair load2D( // load the poses while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; - if ((tag == "VERTEX2") || (tag == "VERTEX")) { + if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { int id; double x, y, yaw; is >> id >> x >> y >> yaw; + // optional filter if (maxID && id >= maxID) continue; + initial->insert(id, Pose2(x, y, yaw)); } is.ignore(LINESIZE, '\n'); @@ -109,54 +198,47 @@ pair load2D( is.clear(); /* clears the end-of-file and error flags */ is.seekg(0, ios::beg); - // Create a sampler with random number generator - Sampler sampler(42u); + // If asked, create a sampler with random number generator + Sampler sampler; + if (addNoise) { + noiseModel::Diagonal::shared_ptr noise; + if (model) + noise = boost::dynamic_pointer_cast(model); + if (!noise) + throw invalid_argument( + "gtsam::load2D: invalid noise model for adding noise" + "(current version assumes diagonal noise model)!"); + sampler = Sampler(noise); + } - // load the factors + // Parse the pose constraints + int id1, id2; bool haveLandmark = false; while (is) { - if(! (is >> tag)) + if (!(is >> tag)) break; - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") + || (tag == "ODOMETRY")) { + + // Read transform double x, y, yaw; - double v1, v2, v3, v4, v5, v6; - is >> id1 >> id2 >> x >> y >> yaw; - is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; + Pose2 l1Xl2(x, y, yaw); - // Try to guess covariance matrix layout - Matrix m(3,3); - if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) - { - // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] - m << v1, v2, v5, v2, v3, v6, v5, v6, v4; - } - else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) - { - // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] - m << v1, v2, v3, v2, v4, v5, v3, v5, v6; - } - else - { - throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file"); - } + // read noise model + SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat, + kernelFunctionType); // optional filter if (maxID && (id1 >= maxID || id2 >= maxID)) continue; - Pose2 l1Xl2(x, y, yaw); - - // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); - if (!model) { - Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2)); - model = noiseModel::Diagonal::Variances(variances, smart); - } + if (!model) + model = modelInFile; if (addNoise) - l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model)); + l1Xl2 = l1Xl2.retract(sampler.sample()); // Insert vertices if pure odometry file if (!initial->exists(id1)) @@ -165,75 +247,84 @@ pair load2D( initial->insert(id2, initial->at(id1) * l1Xl2); NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, *model)); + new BetweenFactor(id1, id2, l1Xl2, model)); graph->push_back(factor); } + + // Parse measurements + double bearing, range, bearing_std, range_std; + + // A bearing-range measurement if (tag == "BR") { - int id1, id2; - double bearing, range, bearing_std, range_std; - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); - - // Insert poses or points if they do not exist yet - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) { - Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); - Point2 global = pose.transform_from(local); - initial->insert(id2, global); - } } + + // A landmark measurement, TODO Frank says: don't know why is converted to bearing-range if (tag == "LANDMARK") { - int id1, id2; double lmx, lmy; double v1, v2, v3; is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range - double bearing = std::atan2(lmy, lmx); - double range = std::sqrt(lmx*lmx + lmy*lmy); + bearing = atan2(lmy, lmx); + range = sqrt(lmx * lmx + lmy * lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume - // that it describes the uncertainty at a range of 10m, and convert that to bearing/range - // uncertainty. - SharedDiagonal measurementNoise; - if(std::abs(v1 - v3) < 1e-4) - { - double rangeVar = v1; - double bearingVar = v1 / 10.0; - measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar)); - } - else - { - if(!haveLandmark) { - cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" - "non-uniform covariance on LANDMARK measurements in this file." << endl; + // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. + if (std::abs(v1 - v3) < 1e-4) { + bearing_std = sqrt(v1 / 10.0); + range_std = sqrt(v1); + } else { + bearing_std = 1; + range_std = 1; + if (!haveLandmark) { + cout + << "Warning: load2D is a very simple dataset loader and is ignoring the\n" + "non-uniform covariance on LANDMARK measurements in this file." + << endl; haveLandmark = true; } } + } + + // Do some common stuff for bearing-range measurements + if (tag == "LANDMARK" || tag == "BR") { + + // optional filter + if (maxID && id1 >= maxID) + continue; + + // Create noise model + noiseModel::Diagonal::shared_ptr measurementNoise = + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); // Add to graph - *graph += BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise); + *graph += BearingRangeFactor(id1, L(id2), bearing, range, + measurementNoise); + + // Insert poses or points if they do not exist yet + if (!initial->exists(id1)) + initial->insert(id1, Pose2()); + if (!initial->exists(L(id2))) { + Pose2 pose = initial->at(id1); + Point2 local(cos(bearing) * range, sin(bearing) * range); + Point2 global = pose.transform_from(local); + initial->insert(L(id2), global); + } } is.ignore(LINESIZE, '\n'); } - cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; - return make_pair(graph, initial); } +/* ************************************************************************* */ +GraphAndValues load2D_robust(const string& filename, + noiseModel::Base::shared_ptr& model, int maxID) { + return load2D(filename, model, maxID); +} + /* ************************************************************************* */ void save2D(const NonlinearFactorGraph& graph, const Values& config, const noiseModel::Diagonal::shared_ptr model, const string& filename) { @@ -241,18 +332,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, fstream stream(filename.c_str(), fstream::out); // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) - { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { const Pose2& pose = dynamic_cast(key_value.value); - stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " - << pose.y() << " " << pose.theta() << endl; + stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() + << " " << pose.theta() << endl; } // save edges Matrix R = model->R(); Matrix RR = trans(R) * R; //prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr factor_, graph) - { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (!factor) @@ -260,14 +349,62 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, Pose2 pose = factor->measured().inverse(); stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " - << pose.x() << " " << pose.y() << " " << pose.theta() << " " - << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " - << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; + << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) + << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " + << RR(0, 2) << " " << RR(1, 2) << endl; } stream.close(); } +/* ************************************************************************* */ +GraphAndValues readG2o(const string& g2oFile, + KernelFunctionType kernelFunctionType) { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); +} + +/* ************************************************************************* */ +void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, + const string& filename) { + + fstream stream(filename.c_str(), fstream::out); + + // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { + const Pose2& pose = dynamic_cast(key_value.value); + stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + << pose.y() << " " << pose.theta() << endl; + } + + // save edges + BOOST_FOREACH(boost::shared_ptr factor_, graph) { + boost::shared_ptr > factor = + boost::dynamic_pointer_cast >(factor_); + if (!factor) + continue; + + SharedNoiseModel model = factor->get_noiseModel(); + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast(model); + if (!diagonalModel) + throw invalid_argument( + "writeG2o: invalid noise model (current version assumes diagonal noise model)!"); + + Pose2 pose = factor->measured(); //.inverse(); + stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " + << pose.x() << " " << pose.y() << " " << pose.theta() << " " + << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " + << diagonalModel->precision(1) << " " << 0.0 << " " + << diagonalModel->precision(2) << endl; + } + stream.close(); +} + /* ************************************************************************* */ bool load3D(const string& filename) { ifstream is(filename.c_str()); @@ -311,161 +448,60 @@ bool load3D(const string& filename) { } /* ************************************************************************* */ -pair load2D_robust( - const string& filename, noiseModel::Base::shared_ptr& model, int maxID) { - cout << "Will try to read " << filename << endl; - ifstream is(filename.c_str()); - if (!is) - throw std::invalid_argument("load2D: can not find the file!"); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); - - string tag; - - // load the poses - while (is) { - is >> tag; - - if ((tag == "VERTEX2") || (tag == "VERTEX")) { - int id; - double x, y, yaw; - is >> id >> x >> y >> yaw; - // optional filter - if (maxID && id >= maxID) - continue; - initial->insert(id, Pose2(x, y, yaw)); - } - is.ignore(LINESIZE, '\n'); - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - - // Create a sampler with random number generator - Sampler sampler(42u); - - // load the factors - while (is) { - is >> tag; - - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; - double x, y, yaw; - - is >> id1 >> id2 >> x >> y >> yaw; - Matrix m = eye(3); - is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2); - m(2, 0) = m(0, 2); - m(2, 1) = m(1, 2); - m(1, 0) = m(0, 1); - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - Pose2 l1Xl2(x, y, yaw); - - // Insert vertices if pure odometry file - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) - initial->insert(id2, initial->at(id1) * l1Xl2); - - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, model)); - graph->push_back(factor); - } - if (tag == "BR") { - int id1, id2; - double bearing, range, bearing_std, range_std; - - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); - - // Insert poses or points if they do not exist yet - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) { - Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); - Point2 global = pose.transform_from(local); - initial->insert(id2, global); - } - } - is.ignore(LINESIZE, '\n'); - } - - cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; - - return make_pair(graph, initial); -} - -/* ************************************************************************* */ -Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL +Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL /* R = [ 1 0 0 * 0 -1 0 * 0 0 -1] */ - Matrix3 R_mat = Matrix3::Zero(3,3); - R_mat(0,0) = 1.0; R_mat(1,1) = -1.0; R_mat(2,2) = -1.0; + Matrix3 R_mat = Matrix3::Zero(3, 3); + R_mat(0, 0) = 1.0; + R_mat(1, 1) = -1.0; + R_mat(2, 2) = -1.0; return Rot3(R_mat); } /* ************************************************************************* */ -Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) -{ +Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) { Rot3 R90 = openGLFixedRotation(); - Rot3 wRc = ( R.inverse() ).compose(R90); + Rot3 wRc = (R.inverse()).compose(R90); // Our camera-to-world translation wTc = -R'*t - return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz))); + return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); } /* ************************************************************************* */ -Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) -{ +Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) { Rot3 R90 = openGLFixedRotation(); - Rot3 cRw_openGL = R90.compose( R.inverse() ); - Point3 t_openGL = cRw_openGL.rotate(Point3(-tx,-ty,-tz)); + Rot3 cRw_openGL = R90.compose(R.inverse()); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); return Pose3(cRw_openGL, t_openGL); } /* ************************************************************************* */ -Pose3 gtsam2openGL(const Pose3& PoseGTSAM) -{ - return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z()); +Pose3 gtsam2openGL(const Pose3& PoseGTSAM) { + return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), + PoseGTSAM.z()); } /* ************************************************************************* */ -bool readBundler(const string& filename, SfM_data &data) -{ +bool readBundler(const string& filename, SfM_data &data) { // Load the data file - ifstream is(filename.c_str(),ifstream::in); - if(!is) - { + ifstream is(filename.c_str(), ifstream::in); + if (!is) { cout << "Error in readBundler: can not find the file!!" << endl; return false; } // Ignore the first line char aux[500]; - is.getline(aux,500); + is.getline(aux, 500); // Get the number of camera poses and 3D points size_t nrPoses, nrPoints; is >> nrPoses >> nrPoints; // Get the information for the camera poses - for( size_t i = 0; i < nrPoses; i++ ) - { + for (size_t i = 0; i < nrPoses; i++) { // Get the focal length and the radial distortion parameters float f, k1, k2; is >> f >> k1 >> k2; @@ -475,20 +511,15 @@ bool readBundler(const string& filename, SfM_data &data) float r11, r12, r13; float r21, r22, r23; float r31, r32, r33; - is >> r11 >> r12 >> r13 - >> r21 >> r22 >> r23 - >> r31 >> r32 >> r33; + is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; // Bundler-OpenGL rotation matrix - Rot3 R( - r11, r12, r13, - r21, r22, r23, - r31, r32, r33); + Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); // Check for all-zero R, in which case quit - if(r11==0 && r12==0 && r13==0) - { - cout << "Error in readBundler: zero rotation matrix for pose " << i << endl; + if (r11 == 0 && r12 == 0 && r13 == 0) { + cout << "Error in readBundler: zero rotation matrix for pose " << i + << endl; return false; } @@ -496,38 +527,36 @@ bool readBundler(const string& filename, SfM_data &data) float tx, ty, tz; is >> tx >> ty >> tz; - Pose3 pose = openGL2gtsam(R,tx,ty,tz); + Pose3 pose = openGL2gtsam(R, tx, ty, tz); - data.cameras.push_back(SfM_Camera(pose,K)); + data.cameras.push_back(SfM_Camera(pose, K)); } // Get the information for the 3D points - for( size_t j = 0; j < nrPoints; j++ ) - { + for (size_t j = 0; j < nrPoints; j++) { SfM_Track track; // Get the 3D position float x, y, z; is >> x >> y >> z; - track.p = Point3(x,y,z); + track.p = Point3(x, y, z); // Get the color information float r, g, b; is >> r >> g >> b; - track.r = r/255.f; - track.g = g/255.f; - track.b = b/255.f; + track.r = r / 255.f; + track.g = g / 255.f; + track.b = b / 255.f; // Now get the visibility information size_t nvisible = 0; is >> nvisible; - for( size_t k = 0; k < nvisible; k++ ) - { + for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.measurements.push_back(make_pair(cam_idx,Point2(u,-v))); + track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); } data.tracks.push_back(track); @@ -538,12 +567,10 @@ bool readBundler(const string& filename, SfM_data &data) } /* ************************************************************************* */ -bool readBAL(const string& filename, SfM_data &data) -{ +bool readBAL(const string& filename, SfM_data &data) { // Load the data file - ifstream is(filename.c_str(),ifstream::in); - if(!is) - { + ifstream is(filename.c_str(), ifstream::in); + if (!is) { cout << "Error in readBAL: can not find the file!!" << endl; return false; } @@ -555,44 +582,41 @@ bool readBAL(const string& filename, SfM_data &data) data.tracks.resize(nrPoints); // Get the information for the observations - for( size_t k = 0; k < nrObservations; k++ ) - { + for (size_t k = 0; k < nrObservations; k++) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.push_back(make_pair(i,Point2(u,-v))); + data.tracks[j].measurements.push_back(make_pair(i, Point2(u, -v))); } // Get the information for the camera poses - for( size_t i = 0; i < nrPoses; i++ ) - { + for (size_t i = 0; i < nrPoses; i++) { // Get the rodriguez vector float wx, wy, wz; is >> wx >> wy >> wz; - Rot3 R = Rot3::rodriguez(wx, wy, wz);// BAL-OpenGL rotation matrix + Rot3 R = Rot3::rodriguez(wx, wy, wz); // BAL-OpenGL rotation matrix // Get the translation vector float tx, ty, tz; is >> tx >> ty >> tz; - Pose3 pose = openGL2gtsam(R,tx,ty,tz); + Pose3 pose = openGL2gtsam(R, tx, ty, tz); // Get the focal length and the radial distortion parameters float f, k1, k2; is >> f >> k1 >> k2; Cal3Bundler K(f, k1, k2); - data.cameras.push_back(SfM_Camera(pose,K)); + data.cameras.push_back(SfM_Camera(pose, K)); } // Get the information for the 3D points - for( size_t j = 0; j < nrPoints; j++ ) - { + for (size_t j = 0; j < nrPoints; j++) { // Get the 3D position float x, y, z; is >> x >> y >> z; SfM_Track& track = data.tracks[j]; - track.p = Point3(x,y,z); + track.p = Point3(x, y, z); track.r = 0.4f; track.g = 0.4f; track.b = 0.4f; @@ -603,8 +627,7 @@ bool readBAL(const string& filename, SfM_data &data) } /* ************************************************************************* */ -bool writeBAL(const string& filename, SfM_data &data) -{ +bool writeBAL(const string& filename, SfM_data &data) { // Open the output file ofstream os; os.open(filename.c_str()); @@ -615,49 +638,55 @@ bool writeBAL(const string& filename, SfM_data &data) } // Write the number of camera poses and 3D points - size_t nrObservations=0; - for (size_t j = 0; j < data.number_tracks(); j++){ + size_t nrObservations = 0; + for (size_t j = 0; j < data.number_tracks(); j++) { nrObservations += data.tracks[j].number_measurements(); } // Write observations - os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl; + os << data.number_cameras() << " " << data.number_tracks() << " " + << nrObservations << endl; os << endl; - for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j SfM_Track track = data.tracks[j]; - for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j + for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); - if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;} + if (u0 != 0 || v0 != 0) { + cout + << "writeBAL has not been tested for calibration with nonzero (u0,v0)" + << endl; + } double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = - (track.measurements[k].second.y() - v0); // center of image is the origin + double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); - os << i /*camera id*/ << " " << j /*point id*/ << " " - << pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl; + os << i /*camera id*/<< " " << j /*point id*/<< " " + << pixelMeasurement.x() /*u of the pixel*/<< " " + << pixelMeasurement.y() /*v of the pixel*/<< endl; } } os << endl; // Write cameras - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera Pose3 poseGTSAM = data.cameras[i].pose(); Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); - os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().vector() << endl; - os << cameraCalibration.fx() << endl; - os << cameraCalibration.k1() << endl; - os << cameraCalibration.k2() << endl; + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().vector() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; os << endl; } // Write the points - for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j Point3 point = data.tracks[j].p; os << point.x() << endl; os << point.y() << endl; @@ -669,48 +698,55 @@ bool writeBAL(const string& filename, SfM_data &data) return true; } -bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){ +bool writeBALfromValues(const string& filename, const SfM_data &data, + Values& values) { SfM_data dataValues = data; // Store poses or cameras in SfM_data Values valuesPoses = values.filter(); - if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses - for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); + if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera + Key poseKey = symbol('x', i); Pose3 pose = values.at(poseKey); Cal3Bundler K = dataValues.cameras[i].calibration(); PinholeCamera camera(pose, K); dataValues.cameras[i] = camera; } } else { - Values valuesCameras = values.filter< PinholeCamera >(); - if ( valuesCameras.size() == dataValues.number_cameras() ){ // we only estimated camera poses and calibration - for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera + Values valuesCameras = values.filter >(); + if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration + for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera Key cameraKey = i; // symbol('c',i); - PinholeCamera camera = values.at >(cameraKey); + PinholeCamera camera = + values.at >(cameraKey); dataValues.cameras[i] = camera; } - }else{ - cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras() - <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; + } else { + cout + << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " + << dataValues.number_cameras() << ") and values (#cameras " + << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" + << endl; return false; } } // Store 3D points in SfM_data Values valuesPoints = values.filter(); - if( valuesPoints.size() != dataValues.number_tracks()){ - cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks() - <<") and values (#points " << valuesPoints.size() << ")!!" << endl; + if (valuesPoints.size() != dataValues.number_tracks()) { + cout + << "writeBALfromValues: different number of points in SfM_dataValues (#points= " + << dataValues.number_tracks() << ") and values (#points " + << valuesPoints.size() << ")!!" << endl; } - for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point - Key pointKey = symbol('l',j); - if(values.exists(pointKey)){ + for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point + Key pointKey = P(j); + if (values.exists(pointKey)) { Point3 point = values.at(pointKey); dataValues.tracks[j].p = point; - }else{ + } else { dataValues.tracks[j].r = 1.0; dataValues.tracks[j].g = 0.0; dataValues.tracks[j].b = 0.0; @@ -726,7 +762,7 @@ Values initialCamerasEstimate(const SfM_data& db) { Values initial; size_t i = 0; // NO POINTS: j = 0; BOOST_FOREACH(const SfM_Camera& camera, db.cameras) - initial.insert(i++, camera); + initial.insert(i++, camera); return initial; } @@ -734,9 +770,9 @@ Values initialCamerasAndPointsEstimate(const SfM_data& db) { Values initial; size_t i = 0, j = 0; BOOST_FOREACH(const SfM_Camera& camera, db.cameras) - initial.insert((i++), camera); + initial.insert((i++), camera); BOOST_FOREACH(const SfM_Track& track, db.tracks) - initial.insert(P(j++), track.p); + initial.insert(P(j++), track.p); return initial; } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 64ccd37b5..8fd75269c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -35,7 +35,7 @@ namespace gtsam { /** * Find the full path to an example dataset distributed with gtsam. The name * may be specified with or without a file extension - if no extension is - * give, this function first looks for the .graph extension, then .txt. We + * given, this function first looks for the .graph extension, then .txt. We * first check the gtsam source tree for the file, followed by the installed * example dataset location. Both the source tree and installed locations * are obtained from CMake during compilation. @@ -44,8 +44,30 @@ namespace gtsam { * search process described above. */ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); + +/** + * Creates a temporary file name that needs to be ignored in .gitingnore + * for checking read-write oprations + */ +GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); #endif +/// Indicates how noise parameters are stored in file +enum NoiseFormat { + NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 + NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr + NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix ! + NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 +}; + +/// Robust kernel type to wrap around quadratic noise model +enum KernelFunctionType { + KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY +}; + +/// Return type for load functions +typedef std::pair GraphAndValues; + /** * Load TORO 2D Graph * @param dataset/model pair as constructed by [dataset] @@ -53,31 +75,57 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ -GTSAM_EXPORT std::pair load2D( - std::pair > dataset, - int maxID = 0, bool addNoise = false, bool smart = true); +GTSAM_EXPORT GraphAndValues load2D( + std::pair dataset, int maxID = 0, + bool addNoise = false, + bool smart = true, // + NoiseFormat noiseFormat = NoiseFormatGRAPH, + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** - * Load TORO 2D Graph + * Load TORO/G2O style graph files * @param filename * @param model optional noise model to use instead of one specified by file * @param maxID if non-zero cut out vertices >= maxID * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model + * @param noiseFormat how noise parameters are stored + * @param kernelFunctionType whether to wrap the noise model in a robust kernel + * @return graph and initial values */ -GTSAM_EXPORT std::pair load2D( - const std::string& filename, - boost::optional model = boost::optional< - noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, - bool smart = true); +GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, + SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise = + false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, // + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); -GTSAM_EXPORT std::pair load2D_robust( - const std::string& filename, - gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0); +/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel +GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, + noiseModel::Base::shared_ptr& model, int maxID = 0); /** save 2d graph */ -GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, - const noiseModel::Diagonal::shared_ptr model, const std::string& filename); +GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, + const Values& config, const noiseModel::Diagonal::shared_ptr model, + const std::string& filename); + +/** + * @brief This function parses a g2o file and stores the measurements into a + * NonlinearFactorGraph and the initial guess in a Values structure + * @param filename The name of the g2o file + * @param kernelFunctionType whether to wrap the noise model in a robust kernel + * @return graph and initial values + */ +GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); + +/** + * @brief This function writes a g2o file from + * NonlinearFactorGraph and a Values structure + * @param filename The name of the g2o file to write + * @param graph NonlinearFactor graph storing the measurements + * @param estimate Values + */ +GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, + const Values& estimate, const std::string& filename); /** * Load TORO 3D Graph @@ -85,27 +133,31 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config GTSAM_EXPORT bool load3D(const std::string& filename); /// A measurement with its camera index -typedef std::pair SfM_Measurement; +typedef std::pair SfM_Measurement; /// Define the structure for the 3D points -struct SfM_Track -{ - gtsam::Point3 p; ///< 3D position of the point - float r,g,b; ///< RGB color of the 3D point +struct SfM_Track { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) - size_t number_measurements() const { return measurements.size();} + size_t number_measurements() const { + return measurements.size(); + } }; /// Define the structure for the camera poses -typedef gtsam::PinholeCamera SfM_Camera; +typedef PinholeCamera SfM_Camera; /// Define the structure for SfM data -struct SfM_data -{ - std::vector cameras; ///< Set of cameras +struct SfM_data { + std::vector cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points - size_t number_cameras() const { return cameras.size();} ///< The number of camera poses - size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points + size_t number_cameras() const { + return cameras.size(); + } ///< The number of camera poses + size_t number_tracks() const { + return tracks.size(); + } ///< The number of reconstructed 3D points }; /** @@ -146,7 +198,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data); * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 * @return true if the parsing was successful, false otherwise */ -GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values); +GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, + const SfM_data &data, Values& values); /** * @brief This function converts an openGL camera pose to an GTSAM camera pose @@ -189,5 +242,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db); */ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db); - } // namespace gtsam diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp new file mode 100644 index 000000000..abb72021a --- /dev/null +++ b/gtsam/slam/lago.cpp @@ -0,0 +1,399 @@ +/* ---------------------------------------------------------------------------- + + * 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 lago.h + * @author Luca Carlone + * @author Frank Dellaert + * @date May 14, 2014 + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { +namespace lago { + +static const Matrix I = eye(1); +static const Matrix I3 = eye(3); + +static const Key keyAnchor = symbol('Z', 9999999); +static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = + noiseModel::Diagonal::Sigmas((Vector(1) << 0)); +static const noiseModel::Diagonal::shared_ptr priorPose2Noise = + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + +/* ************************************************************************* */ +/** + * Compute the cumulative orientation (without wrapping) wrt the root of a + * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and + * moves towards the root summing up the (directed) rotation measurements. + * Relative measurements are encoded in "deltaThetaMap". + * The root is assumed to have orientation zero. + */ +static double computeThetaToRoot(const Key nodeKey, + const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, + const key2doubleMap& thetaFromRootMap) { + + double nodeTheta = 0; + Key key_child = nodeKey; // the node + Key key_parent = 0; // the initialization does not matter + while (1) { + // We check if we reached the root + if (tree.at(key_child) == key_child) // if we reached the root + break; + // we sum the delta theta corresponding to the edge parent->child + nodeTheta += deltaThetaMap.at(key_child); + // we get the parent + key_parent = tree.at(key_child); // the parent + // we check if we connected to some part of the tree we know + if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) { + nodeTheta += thetaFromRootMap.at(key_parent); + break; + } + key_child = key_parent; // we move upwards in the tree + } + return nodeTheta; +} + +/* ************************************************************************* */ +key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, + const PredecessorMap& tree) { + + key2doubleMap thetaToRootMap; + + // Orientation of the roo + thetaToRootMap.insert(pair(keyAnchor, 0.0)); + + // for all nodes in the tree + BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { + // compute the orientation wrt root + Key nodeKey = it.first; + double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, + thetaToRootMap); + thetaToRootMap.insert(pair(nodeKey, nodeTheta)); + } + return thetaToRootMap; +} + +/* ************************************************************************* */ +void getSymbolicGraph( +/*OUTPUTS*/vector& spanningTreeIds, vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) { + + // Get keys for which you want the orientation + size_t id = 0; + // Loop over the factors + BOOST_FOREACH(const boost::shared_ptr& factor, g) { + if (factor->keys().size() == 2) { + Key key1 = factor->keys()[0]; + Key key2 = factor->keys()[1]; + // recast to a between + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + if (!pose2Between) + continue; + // get the orientation - measured().theta(); + double deltaTheta = pose2Between->measured().theta(); + // insert (directed) orientations in the map "deltaThetaMap" + bool inTree = false; + if (tree.at(key1) == key2) { // key2 -> key1 + deltaThetaMap.insert(pair(key1, -deltaTheta)); + inTree = true; + } else if (tree.at(key2) == key1) { // key1 -> key2 + deltaThetaMap.insert(pair(key2, deltaTheta)); + inTree = true; + } + // store factor slot, distinguishing spanning tree edges from chordsIds + if (inTree == true) + spanningTreeIds.push_back(id); + else + // it's a chord! + chordsIds.push_back(id); + } + id++; + } +} + +/* ************************************************************************* */ +// Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor +static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, + Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { + + // Get the relative rotation measurement from the between factor + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + if (!pose2Between) + throw invalid_argument( + "buildLinearOrientationGraph: invalid between factor!"); + deltaTheta = (Vector(1) << pose2Between->measured().theta()); + + // Retrieve the noise model for the relative rotation + SharedNoiseModel model = pose2Between->get_noiseModel(); + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast(model); + if (!diagonalModel) + throw invalid_argument("buildLinearOrientationGraph: invalid noise model " + "(current version assumes diagonal noise model)!"); + Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement + model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); +} + +/* ************************************************************************* */ +GaussianFactorGraph buildLinearOrientationGraph( + const vector& spanningTreeIds, const vector& chordsIds, + const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, + const PredecessorMap& tree) { + + GaussianFactorGraph lagoGraph; + Vector deltaTheta; + noiseModel::Diagonal::shared_ptr model_deltaTheta; + + // put original measurements in the spanning tree + BOOST_FOREACH(const size_t& factorId, spanningTreeIds) { + const FastVector& keys = g[factorId]->keys(); + Key key1 = keys[0], key2 = keys[1]; + getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); + lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta); + } + // put regularized measurements in the chordsIds + BOOST_FOREACH(const size_t& factorId, chordsIds) { + const FastVector& keys = g[factorId]->keys(); + Key key1 = keys[0], key2 = keys[1]; + getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); + double key1_DeltaTheta_key2 = deltaTheta(0); + ///cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << endl; + double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) + - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord + double k = boost::math::round(k2pi_noise / (2 * M_PI)); + //if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug + Vector deltaThetaRegularized = (Vector(1) + << key1_DeltaTheta_key2 - 2 * k * M_PI); + lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta); + } + // prior on the anchor orientation + lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise); + return lagoGraph; +} + +/* ************************************************************************* */ +// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node +static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { + gttic(lago_buildPose2graph); + NonlinearFactorGraph pose2Graph; + + BOOST_FOREACH(const boost::shared_ptr& factor, graph) { + + // recast to a between on Pose2 + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + if (pose2Between) + pose2Graph.add(pose2Between); + + // recast PriorFactor to BetweenFactor + boost::shared_ptr > pose2Prior = + boost::dynamic_pointer_cast >(factor); + if (pose2Prior) + pose2Graph.add( + BetweenFactor(keyAnchor, pose2Prior->keys()[0], + pose2Prior->prior(), pose2Prior->get_noiseModel())); + } + return pose2Graph; +} + +/* ************************************************************************* */ +static PredecessorMap findOdometricPath( + const NonlinearFactorGraph& pose2Graph) { + + PredecessorMap tree; + Key minKey; + bool minUnassigned = true; + + BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph) { + + Key key1 = std::min(factor->keys()[0], factor->keys()[1]); + Key key2 = std::max(factor->keys()[0], factor->keys()[1]); + if (minUnassigned) { + minKey = key1; + minUnassigned = false; + } + if (key2 - key1 == 1) { // consecutive keys + tree.insert(key2, key1); + if (key1 < minKey) + minKey = key1; + } + } + tree.insert(minKey, keyAnchor); + tree.insert(keyAnchor, keyAnchor); // root + return tree; +} + +/* ************************************************************************* */ +// Return the orientations of a graph including only BetweenFactors +static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, + bool useOdometricPath) { + gttic(lago_computeOrientations); + + // Find a minimum spanning tree + PredecessorMap tree; + if (useOdometricPath) + tree = findOdometricPath(pose2Graph); + else + tree = findMinimumSpanningTree >(pose2Graph); + + // Create a linear factor graph (LFG) of scalars + key2doubleMap deltaThetaMap; + vector spanningTreeIds; // ids of between factors forming the spanning tree T + vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); + + // temporary structure to correct wraparounds along loops + key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); + + // regularize measurements and plug everything in a factor graph + GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, + chordsIds, pose2Graph, orientationsToRoot, tree); + + // Solve the LFG + VectorValues orientationsLago = lagoGraph.optimize(); + + return orientationsLago; +} + +/* ************************************************************************* */ +VectorValues initializeOrientations(const NonlinearFactorGraph& graph, + bool useOdometricPath) { + + // We "extract" the Pose2 subgraph of the original graph: this + // is done to properly model priors and avoiding operating on a larger graph + NonlinearFactorGraph pose2Graph = buildPose2graph(graph); + + // Get orientations from relative orientation measurements + return computeOrientations(pose2Graph, useOdometricPath); +} + +/* ************************************************************************* */ +Values computePoses(const NonlinearFactorGraph& pose2graph, + VectorValues& orientationsLago) { + gttic(lago_computePoses); + + // Linearized graph on full poses + GaussianFactorGraph linearPose2graph; + + // We include the linear version of each between factor + BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { + + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + + if (pose2Between) { + Key key1 = pose2Between->keys()[0]; + double theta1 = orientationsLago.at(key1)(0); + double s1 = sin(theta1); + double c1 = cos(theta1); + + Key key2 = pose2Between->keys()[1]; + double theta2 = orientationsLago.at(key2)(0); + + double linearDeltaRot = theta2 - theta1 + - pose2Between->measured().theta(); + linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize + + double dx = pose2Between->measured().x(); + double dy = pose2Between->measured().y(); + + Vector globalDeltaCart = // + (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy); + Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs + Matrix J1 = -I3; + J1(0, 2) = s1 * dx + c1 * dy; + J1(1, 2) = -c1 * dx + s1 * dy; + // Retrieve the noise model for the relative rotation + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast( + pose2Between->get_noiseModel()); + + linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); + } else { + throw invalid_argument( + "computeLagoPoses: cannot manage non between factor here!"); + } + } + // add prior + linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), + priorPose2Noise); + + // optimize + VectorValues posesLago = linearPose2graph.optimize(); + + // put into Values structure + Values initialGuessLago; + BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { + Key key = it.first; + if (key != keyAnchor) { + const Vector& poseVector = it.second; + Pose2 poseLago = Pose2(poseVector(0), poseVector(1), + orientationsLago.at(key)(0) + poseVector(2)); + initialGuessLago.insert(key, poseLago); + } + } + return initialGuessLago; +} + +/* ************************************************************************* */ +Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { + gttic(lago_initialize); + + // We "extract" the Pose2 subgraph of the original graph: this + // is done to properly model priors and avoiding operating on a larger graph + NonlinearFactorGraph pose2Graph = buildPose2graph(graph); + + // Get orientations from relative orientation measurements + VectorValues orientationsLago = computeOrientations(pose2Graph, + useOdometricPath); + + // Compute the full poses + return computePoses(pose2Graph, orientationsLago); +} + +/* ************************************************************************* */ +Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess) { + Values initialGuessLago; + + // get the orientation estimates from LAGO + VectorValues orientations = initializeOrientations(graph); + + // for all nodes in the tree + BOOST_FOREACH(const VectorValues::value_type& it, orientations) { + Key key = it.first; + if (key != keyAnchor) { + const Pose2& pose = initialGuess.at(key); + const Vector& orientation = it.second; + Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); + initialGuessLago.insert(key, poseLago); + } + } + return initialGuessLago; +} + +} // end of namespace lago +} // end of namespace gtsam diff --git a/gtsam/slam/lago.h b/gtsam/slam/lago.h new file mode 100644 index 000000000..0df80ac42 --- /dev/null +++ b/gtsam/slam/lago.h @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------------- + + * 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 lago.h + * @brief Initialize Pose2 in a factor graph using LAGO + * (Linear Approximation for Graph Optimization). see papers: + * + * L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate + * approximation for planar pose graph optimization, IJRR, 2014. + * + * L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation + * for graph-based simultaneous localization and mapping, RSS, 2011. + * + * @param graph: nonlinear factor graph (can include arbitrary factors but we assume + * that there is a subgraph involving Pose2 and betweenFactors). Also in the current + * version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) + * and a prior on x0. This assumption can be relaxed by using the extra argument + * useOdometricPath = false, although this part of code is not stable yet. + * @return Values: initial guess from LAGO (only pose2 are initialized) + * + * @author Luca Carlone + * @author Frank Dellaert + * @date May 14, 2014 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { +namespace lago { + +typedef std::map key2doubleMap; + +/** + * Compute the cumulative orientations (without wrapping) + * for all nodes wrt the root (root has zero orientation). + */ +GTSAM_EXPORT key2doubleMap computeThetasToRoot( + const key2doubleMap& deltaThetaMap, const PredecessorMap& tree); + +/** + * Given a factor graph "g", and a spanning tree "tree", select the nodes belonging + * to the tree and to g, and stores the factor slots corresponding to edges in the + * tree and to chordsIds wrt this tree. + * Also it computes deltaThetaMap which is a fast way to encode relative + * orientations along the tree: for a node key2, s.t. tree[key2]=key1, + * the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1] + */ +GTSAM_EXPORT void getSymbolicGraph( +/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); + +/** Linear factor graph with regularized orientation measurements */ +GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( + const std::vector& spanningTreeIds, + const std::vector& chordsIds, const NonlinearFactorGraph& g, + const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); + +/** LAGO: Return the orientations of the Pose2 in a generic factor graph */ +GTSAM_EXPORT VectorValues initializeOrientations( + const NonlinearFactorGraph& graph, bool useOdometricPath = true); + +/** Return the values for the Pose2 in a generic factor graph */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + bool useOdometricPath = true); + +/** Only correct the orientation part in initialGuess */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess); + +} // end of namespace lago +} // end of namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index f9a2cb34f..d7adf9b51 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -19,10 +19,14 @@ #include +#include #include #include + +#include #include +using namespace gtsam::symbol_shorthand; using namespace std; using namespace gtsam; @@ -35,6 +39,23 @@ TEST(dataSet, findExampleDataFile) { EXPECT(assert_equal(expected_end, actual_end)); } +/* ************************************************************************* */ +TEST( dataSet, load2D) +{ + ///< The structure where we will save the SfM data + const string filename = findExampleDataFile("w100.graph"); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = load2D(filename); + EXPECT_LONGS_EQUAL(300,graph->size()); + EXPECT_LONGS_EQUAL(100,initial->size()); + noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); + BetweenFactor expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model); + BetweenFactor::shared_ptr actual = boost::dynamic_pointer_cast< + BetweenFactor >(graph->at(0)); + EXPECT(assert_equal(expected, *actual)); +} + /* ************************************************************************* */ TEST( dataSet, Balbianello) { @@ -56,6 +77,117 @@ TEST( dataSet, Balbianello) EXPECT(assert_equal(expected,actual,1)); } +/* ************************************************************************* */ +TEST( dataSet, readG2o) +{ + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile); + + Values expectedValues; + expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000)); + expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596)); + expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887)); + expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514)); + expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715)); + expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785)); + expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333)); + expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169)); + expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391)); + expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016)); + expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934)); + EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); + + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + NonlinearFactorGraph expectedGraph; + expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); +} + +/* ************************************************************************* */ +TEST( dataSet, readG2oHuber) +{ + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER); + + noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); + + NonlinearFactorGraph expectedGraph; + expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); +} + +/* ************************************************************************* */ +TEST( dataSet, readG2oTukey) +{ + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY); + + noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); + + NonlinearFactorGraph expectedGraph; + expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); +} + +/* ************************************************************************* */ +TEST( dataSet, writeG2o) +{ + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph::shared_ptr expectedGraph; + Values::shared_ptr expectedValues; + boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile); + + const string filenameToWrite = createRewrittenFileName(g2oFile); + writeG2o(*expectedGraph, *expectedValues, filenameToWrite); + + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite); + EXPECT(assert_equal(*expectedValues,*actualValues,1e-5)); + EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5)); +} + /* ************************************************************************* */ TEST( dataSet, readBAL_Dubrovnik) { @@ -120,7 +252,7 @@ TEST( dataSet, writeBAL_Dubrovnik) readBAL(filenameToRead, readData); // Write readData to file filenameToWrite - const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); + const string filenameToWrite = createRewrittenFileName(filenameToRead); CHECK(writeBAL(filenameToWrite, readData)); // Read what we wrote @@ -176,13 +308,13 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ value.insert(poseKey, pose); } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point - Key pointKey = symbol('l',j); + Key pointKey = P(j); Point3 point = poseChange.transform_from( readData.tracks[j].p ); value.insert(pointKey, point); } // Write values and readData to a file - const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); + const string filenameToWrite = createRewrittenFileName(filenameToRead); writeBALfromValues(filenameToWrite, readData, value); // Read the file we wrote @@ -208,7 +340,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); Point3 expectedPoint = track0.p; - Key pointKey = symbol('l',0); + Key pointKey = P(0); Point3 actualPoint = value.at(pointKey); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); } diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp new file mode 100644 index 000000000..5e4b2faf2 --- /dev/null +++ b/gtsam/slam/tests/testLago.cpp @@ -0,0 +1,326 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPlanarSLAMExample_lago.cpp + * @brief Unit tests for planar SLAM example using the initialization technique + * LAGO (Linear Approximation for Graph Optimization) + * + * @author Luca Carlone + * @author Frank Dellaert + * @date May 14, 2014 + */ + +#include +#include +#include +#include +#include + +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); + +namespace simple { +// We consider a small graph: +// symbolic FG +// x2 0 1 +// / | \ 1 2 +// / | \ 2 3 +// x3 | x1 2 0 +// \ | / 0 3 +// \ | / +// x0 +// + +static Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000); +static Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796); +static Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593); +static Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389); + +NonlinearFactorGraph graph() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), model)); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), model)); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), model)); + g.add(BetweenFactor(x2, x0, pose2.between(pose0), model)); + g.add(BetweenFactor(x0, x3, pose0.between(pose3), model)); + g.add(PriorFactor(x0, pose0, model)); + return g; +} +} + +/* *************************************************************************** */ +TEST( Lago, checkSTandChords ) { + NonlinearFactorGraph g = simple::graph(); + PredecessorMap tree = findMinimumSpanningTree >(g); + + lago::key2doubleMap deltaThetaMap; + vector spanningTreeIds; // ids of between factors forming the spanning tree T + vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + + DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1) + DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0) + DOUBLES_EQUAL(spanningTreeIds[2], 4, 1e-6); // factor 4 is the third in the ST(0->3) + +} + +/* *************************************************************************** */ +TEST( Lago, orientationsOverSpanningTree ) { + NonlinearFactorGraph g = simple::graph(); + PredecessorMap tree = findMinimumSpanningTree >(g); + + // check the tree structure + EXPECT_LONGS_EQUAL(tree[x0], x0); + EXPECT_LONGS_EQUAL(tree[x1], x0); + EXPECT_LONGS_EQUAL(tree[x2], x0); + EXPECT_LONGS_EQUAL(tree[x3], x0); + + lago::key2doubleMap expected; + expected[x0]= 0; + expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1)) + expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) + expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3)) + + lago::key2doubleMap deltaThetaMap; + vector spanningTreeIds; // ids of between factors forming the spanning tree T + vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + + lago::key2doubleMap actual; + actual = lago::computeThetasToRoot(deltaThetaMap, tree); + DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6); + DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6); + DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6); + DOUBLES_EQUAL(expected[x3], actual[x3], 1e-6); +} + +/* *************************************************************************** */ +TEST( Lago, regularizedMeasurements ) { + NonlinearFactorGraph g = simple::graph(); + PredecessorMap tree = findMinimumSpanningTree >(g); + + lago::key2doubleMap deltaThetaMap; + vector spanningTreeIds; // ids of between factors forming the spanning tree T + vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + + lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree); + + GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); + std::pair actualAb = lagoGraph.jacobian(); + // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded) + Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)); + // this is the whitened error, so we multiply by the std to unwhiten + actual = 0.1 * actual; + // Expected regularized measurements (same for the spanning tree, corrected for the chordsIds) + Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2); + + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, smallGraphVectorValues ) { + bool useOdometricPath = false; + VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, smallGraphVectorValuesSP ) { + + VectorValues initial = lago::initializeOrientations(simple::graph()); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePosePriors ) { + bool useOdometricPath = false; + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1, model)); + VectorValues initial = lago::initializeOrientations(g, useOdometricPath); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePosePriorsSP ) { + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1, model)); + VectorValues initial = lago::initializeOrientations(g); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePoseAndRotPriors ) { + bool useOdometricPath = false; + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1.theta(), model)); + VectorValues initial = lago::initializeOrientations(g, useOdometricPath); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePoseAndRotPriorsSP ) { + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1.theta(), model)); + VectorValues initial = lago::initializeOrientations(g); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, smallGraphValues ) { + + // we set the orientations in the initial guess to zero + Values initialGuess; + initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0)); + initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0)); + initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0)); + initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0)); + + // lago does not touch the Cartesian part and only fixed the orientations + Values actual = lago::initialize(simple::graph(), initialGuess); + + // we are in a noiseless case + Values expected; + expected.insert(x0,simple::pose0); + expected.insert(x1,simple::pose1); + expected.insert(x2,simple::pose2); + expected.insert(x3,simple::pose3); + + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, smallGraph2 ) { + + // lago does not touch the Cartesian part and only fixed the orientations + Values actual = lago::initialize(simple::graph()); + + // we are in a noiseless case + Values expected; + expected.insert(x0,simple::pose0); + expected.insert(x1,simple::pose1); + expected.insert(x2,simple::pose2); + expected.insert(x3,simple::pose3); + + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, largeGraphNoisy_orientations ) { + + string inputFile = findExampleDataFile("noisyToyGraph"); + NonlinearFactorGraph::shared_ptr g; + Values::shared_ptr initial; + boost::tie(g, initial) = readG2o(inputFile); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = *g; + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + + VectorValues actualVV = lago::initializeOrientations(graphWithPrior); + Values actual; + Key keyAnc = symbol('Z',9999999); + for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ + Key key = it->first; + if (key != keyAnc){ + Vector orientation = actualVV.at(key); + Pose2 poseLago = Pose2(0.0,0.0,orientation(0)); + actual.insert(key, poseLago); + } + } + string matlabFile = findExampleDataFile("orientationsNoisyToyGraph"); + NonlinearFactorGraph::shared_ptr gmatlab; + Values::shared_ptr expected; + boost::tie(gmatlab, expected) = readG2o(matlabFile); + + BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ + Key k = key_val.key; + EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-5)); + } +} + +/* *************************************************************************** */ +TEST( Lago, largeGraphNoisy ) { + + string inputFile = findExampleDataFile("noisyToyGraph"); + NonlinearFactorGraph::shared_ptr g; + Values::shared_ptr initial; + boost::tie(g, initial) = readG2o(inputFile); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = *g; + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + + Values actual = lago::initialize(graphWithPrior); + + string matlabFile = findExampleDataFile("optimizedNoisyToyGraph"); + NonlinearFactorGraph::shared_ptr gmatlab; + Values::shared_ptr expected; + boost::tie(gmatlab, expected) = readG2o(matlabFile); + + BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ + Key k = key_val.key; + EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-2)); + } +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp similarity index 78% rename from gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp rename to gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 9a3fe7f62..29b482861 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -52,9 +52,9 @@ using symbol_shorthand::X; using symbol_shorthand::L; // tests data -Symbol x1('X', 1); -Symbol x2('X', 2); -Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); @@ -369,6 +369,271 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ if(isDebugTest) tictoc_print_(); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianSVD ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, landmarkDistance ){ + + double excludeLandmarksFutherThanDist = 2; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3),result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + Point3 landmark4(5, -0.5, 1); + + vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, + JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor3->add(measurements_cam3, views, model, K); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor4->add(measurements_cam4, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianQ ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; diff --git a/gtsam/slam/tests/timeLago.cpp b/gtsam/slam/tests/timeLago.cpp new file mode 100644 index 000000000..ff60c4a27 --- /dev/null +++ b/gtsam/slam/tests/timeLago.cpp @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeVirtual.cpp + * @brief Time the overhead of using virtual destructors and methods + * @author Richard Roberts + * @date Dec 3, 2010 + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char *argv[]) { + + size_t trials = 1; + + // read graph + Values::shared_ptr solution; + NonlinearFactorGraph::shared_ptr g; + string inputFile = findExampleDataFile("w10000"); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); + boost::tie(g, solution) = load2D(inputFile, model); + + // add noise to create initial estimate + Values initial; + Sampler sampler(42u); + Values::ConstFiltered poses = solution->filter(); + SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0)); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, poses) + initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); + + // Add prior on the pose having index (key) = 0 + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8)); + g->add(PriorFactor(0, Pose2(), priorModel)); + + // LAGO + for (size_t i = 0; i < trials; i++) { + { + gttic_(lago); + + gttic_(init); + Values lagoInitial = lago::initialize(*g); + gttoc_(init); + + gttic_(refine); + GaussNewtonOptimizer optimizer(*g, lagoInitial); + Values result = optimizer.optimize(); + gttoc_(refine); + } + + { + gttic_(optimize); + GaussNewtonOptimizer optimizer(*g, initial); + Values result = optimizer.optimize(); + } + + tictoc_finishedIteration_(); + } + + tictoc_print_(); + + return 0; +} diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp new file mode 100644 index 000000000..40a0a8725 --- /dev/null +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + +* 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 SmartProjectionFactorExample.cpp +* @brief A stereo visual odometry example +* @date May 30, 2014 +* @author Stephen Camp +* @author Chris Beall +*/ + + +/** + * A smart projection factor example based on stereo data, throwing away the + * measurement from the right camera + * -robot starts at origin + * -moves forward, taking periodic stereo measurements + * -makes monocular observations of many landmarks + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv){ + + typedef SmartProjectionPoseFactor SmartFactor; + + Values initial_estimate; + NonlinearFactorGraph graph; + const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1); + + string calibration_loc = findExampleDataFile("VO_calibration.txt"); + string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); + string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); + + //read camera calibration info from file + // focal lengths fx, fy, skew s, principal point u0, v0, baseline b + cout << "Reading calibration info" << endl; + ifstream calibration_file(calibration_loc.c_str()); + + double fx, fy, s, u0, v0, b; + calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; + const Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u0, v0)); + + cout << "Reading camera poses" << endl; + ifstream pose_file(pose_loc.c_str()); + + int pose_id; + MatrixRowMajor m(4,4); + //read camera pose parameters and use to make initial estimates of camera poses + while (pose_file >> pose_id) { + for (int i = 0; i < 16; i++) { + pose_file >> m.data()[i]; + } + initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + } + + // camera and landmark keys + size_t x, l; + + // pixel coordinates uL, uR, v (same for left/right images due to rectification) + // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + double uL, uR, v, X, Y, Z; + ifstream factor_file(factor_loc.c_str()); + cout << "Reading stereo factors" << endl; + + //read stereo measurements and construct smart factors + + SmartFactor::shared_ptr factor(new SmartFactor()); + size_t current_l = 3; // hardcoded landmark ID from first measurement + + while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + + if(current_l != l) { + graph.push_back(factor); + factor = SmartFactor::shared_ptr(new SmartFactor()); + current_l = l; + } + factor->add(Point2(uL,v), Symbol('x',x), model, K); + } + + Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + //constrain the first pose such that it cannot change from its original value during optimization + // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky + // QR is much slower than Cholesky, but numerically more stable + graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + + cout << "Optimizing" << endl; + //create Levenberg-Marquardt optimizer to optimize the factor graph + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + Values result = optimizer.optimize(); + + cout << "Final result sample:" << endl; + Values pose_values = result.filter(); + pose_values.print("Final camera poses:\n"); + + return 0; +} diff --git a/matlab.h b/matlab.h index 100f5a242..c4891a615 100644 --- a/matlab.h +++ b/matlab.h @@ -28,130 +28,228 @@ #include #include +#include + #include namespace gtsam { - namespace utilities { - - /// Extract all Point2 values into a single matrix [x y] - Matrix extractPoint2(const Values& values) { - size_t j=0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Point3 values into a single matrix [x y z] - Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Pose2 values into a single matrix [x y theta] - Matrix extractPose2(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) - result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); - return result; - } - - /// Extract all Pose3 values - Values allPose3s(const Values& values) { - return values.filter(); - } - - /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] - Matrix extractPose3(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),12); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { - result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); - result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); - result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation().vector(); - j++; - } - return result; - } - - /// Perturb all Point2 values using normally distributed noise - void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Pose2 values using normally distributed noise - void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( - Vector3(sigmaT, sigmaT, sigmaR)); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Point3 values using normally distributed noise - void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Insert a number of initial point values by backprojecting - void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k > - (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); - } - } - - /// Calculate the errors of all projection factors in a graph - Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) - if (boost::dynamic_pointer_cast >(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { - boost::shared_ptr > p = boost::dynamic_pointer_cast >(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - - } +namespace utilities { +// Create a KeyList from indices +FastList createKeyList(const Vector& I) { + FastList set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyList from indices using symbol +FastList createKeyList(string s, const Vector& I) { + FastList set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeyVector from indices +FastVector createKeyVector(const Vector& I) { + FastVector set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyVector from indices using symbol +FastVector createKeyVector(string s, const Vector& I) { + FastVector set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeySet from indices +FastSet createKeySet(const Vector& I) { + FastSet set; + for (int i = 0; i < I.size(); i++) + set.insert(I[i]); + return set; +} + +// Create a KeySet from indices using symbol +FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.insert(symbol(c, I[i])); + return set; +} + +/// Extract all Point2 values into a single matrix [x y] +Matrix extractPoint2(const Values& values) { + size_t j = 0; + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 2); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Point3 values into a single matrix [x y z] +Matrix extractPoint3(const Values& values) { + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Pose2 values into a single matrix [x y theta] +Matrix extractPose2(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + return result; +} + +/// Extract all Pose3 values +Values allPose3s(const Values& values) { + return values.filter(); +} + +/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +Matrix extractPose3(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 12); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.value.translation().vector(); + j++; + } + return result; +} + +/// Perturb all Point2 values using normally distributed noise +void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Pose2 values using normally distributed noise +void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = + 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Point3 values using normally distributed noise +void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Insert a number of initial point values by backprojecting +void insertBackprojections(Values& values, const SimpleCamera& camera, + const Vector& J, const Matrix& Z, double depth) { + if (Z.rows() != 2) + throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "insertBackProjections: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + Point2 p(Z(0, k), Z(1, k)); + Point3 P = camera.backproject(p, depth); + values.insert(J(k), P); + } +} + +/// Insert multiple projection factors for a single pose key +void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, + const Vector& J, const Matrix& Z, const SharedNoiseModel& model, + const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { + if (Z.rows() != 2) + throw std::invalid_argument("addMeasurements: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "addMeasurements: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + graph.push_back( + boost::make_shared >( + Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); + } +} + +/// Calculate the errors of all projection factors in a graph +Matrix reprojectionErrors(const NonlinearFactorGraph& graph, + const Values& values) { + // first count + size_t K = 0, k = 0; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) + if (boost::dynamic_pointer_cast >( + f)) + ++K; + // now fill + Matrix errors(2, K); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + boost::shared_ptr > p = + boost::dynamic_pointer_cast >( + f); + if (p) + errors.col(k++) = p->unwhitenedError(values); + } + return errors; +} + +/// Convert from local to world coordinates +Values localToWorld(const Values& local, const Pose2& base, + const FastVector user_keys = FastVector()) { + + Values world; + + // if no keys given, get all keys from local values + FastVector keys(user_keys); + if (keys.size()==0) + keys = FastVector(local.keys()); + + // Loop over all keys + BOOST_FOREACH(Key key, keys) { + try { + // if value is a Pose2, compose it with base pose + Pose2 pose = local.at(key); + world.insert(key, base.compose(pose)); + } catch (std::exception e1) { + try { + // if value is a Point2, transform it from base pose + Point2 point = local.at(key); + world.insert(key, base.transform_from(point)); + } catch (std::exception e2) { + // if not Pose2 or Point2, do nothing + } + } + } + return world; +} + +} } diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 5fec7721c..023c61dbe 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -175,6 +175,9 @@ % symbolIndex - get index from a symbol key % %% Wrapped C++ Convenience Functions for use within MATLAB +% utilities.createKeyList - Create KeyList from indices +% utilities.createKeyVector - Create KeyVector from indices +% utilities.createKeySet - Create KeySet from indices % utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y] % utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z] % utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta] @@ -186,3 +189,4 @@ % utilities.insertBackprojections - Insert a number of initial point values by backprojecting % utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key % utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph +% utilities.localToWorld - Convert from local to world coordinates diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m index c6b0f85aa..45e7fe467 100644 --- a/matlab/+gtsam/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -8,53 +8,41 @@ function plot2DTrajectory(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'k*-'; + linespec = 'k*-'; end haveMarginals = exist('marginals', 'var'); -keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot poses and covariance matrices -lastIndex = []; -for i = 0:keys.size-1 +% Do something very efficient to draw trajectory +poses = utilities.extractPose2(values); +X = poses(:,1); +Y = poses(:,2); +theta = poses(:,3); +plot(X,Y,linespec); + +% Quiver can also be vectorized if no marginals asked +if ~haveMarginals + C = cos(theta); + S = sin(theta); + quiver(X,Y,C,S,0.1,linespec); +else + % plotPose2 does both quiver and covariance matrix + keys = KeyVector(values.keys); + for i = 0:keys.size-1 key = keys.at(i); x = values.at(key); if isa(x, 'gtsam.Pose2') - if ~isempty(lastIndex) - % Draw line from last pose then covariance ellipse on top of - % last pose. - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); - end - - end - lastIndex = i; - end -end - -% Draw final covariance ellipse -if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); + P = marginals.marginalCovariance(key); + gtsam.plotPose2(x,linespec(1), P); end + end end if ~holdstate - hold off + hold off end end diff --git a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m index 343dee05b..64cda5afc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m @@ -59,16 +59,16 @@ params.setAbsoluteErrorTol(1e-15); params.setRelativeErrorTol(1e-15); params.setVerbosity('ERROR'); params.setVerbosityDL('VERBOSE'); -params.setOrdering(graph.orderingCOLAMD(initialEstimate)); +params.setOrdering(graph.orderingCOLAMD()); optimizer = DoglegOptimizer(graph, initialEstimate, params); result = optimizer.optimizeSafely(); result.print('final result'); %% Get the corresponding dense matrix -ord = graph.orderingCOLAMD(result); -gfg = graph.linearize(result,ord); -denseAb = gfg.denseJacobian; +ord = graph.orderingCOLAMD(); +gfg = graph.linearize(result); +denseAb = gfg.augmentedJacobian; %% Get sparse matrix A and RHS b IJS = gfg.sparseJacobian_(); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index b4957cce3..83ec949cc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -36,7 +36,9 @@ toc hold on; plot2DTrajectory(result, 'b-*'); %% Plot Covariance Ellipses +tic marginals = Marginals(graph, result); +toc P={}; for i=1:result.size()-1 pose_i = result.at(i); diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 01df4fc33..39e48c204 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -12,6 +12,8 @@ import gtsam.* +%% PLEASE NOTE THAT PLOTTING TAKES A VERY LONG TIME HERE + %% Find data file N = 2500; % dataset = 'sphere_smallnoise.graph'; diff --git a/matlab/gtsam_tests/testUtilities.m b/matlab/gtsam_tests/testUtilities.m new file mode 100644 index 000000000..da8dec789 --- /dev/null +++ b/matlab/gtsam_tests/testUtilities.m @@ -0,0 +1,47 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Checks for results of functions in utilities namespace +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Create keys for variables +x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3); + +actual = utilities.createKeyList([1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==1', actual.front==1); + +actual = utilities.createKeyList('x',[1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==x1', actual.front==x1); + +actual = utilities.createKeyVector([1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==1', actual.at(0)==1); + +actual = utilities.createKeyVector('x',[1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==x1', actual.at(0)==x1); + +actual = utilities.createKeySet([1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(1)', actual.count(1)); + +actual = utilities.createKeySet('x',[1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(x1)', actual.count(x1)); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 2cad40df8..c379179c5 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -33,5 +33,8 @@ testVisualISAMExample display 'Starting: testSerialization' testSerialization +display 'Starting: testUtilities' +testUtilities + % end of tests display 'Tests complete!' diff --git a/package_scripts/compile_static_boost.sh b/package_scripts/compile_static_boost.sh old mode 100644 new mode 100755 diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh index 7ae0fa169..3092fe6a3 100755 --- a/package_scripts/toolbox_package_unix.sh +++ b/package_scripts/toolbox_package_unix.sh @@ -33,22 +33,21 @@ fi # Run cmake cmake -DCMAKE_BUILD_TYPE=Release \ --DGTSAM_INSTALL_MATLAB_TOOLBOX:bool=true \ +-DGTSAM_INSTALL_MATLAB_TOOLBOX:BOOL=ON \ -DCMAKE_INSTALL_PREFIX="$PWD/stage" \ --DBoost_NO_SYSTEM_PATHS:bool=true \ --DBoost_USE_STATIC_LIBS:bool=true \ +-DBoost_NO_SYSTEM_PATHS:BOOL=ON \ +-DBoost_USE_STATIC_LIBS:BOOL=ON \ -DBOOST_ROOT="$1" \ --DGTSAM_BUILD_SHARED_LIBRARY:bool=false \ --DGTSAM_BUILD_STATIC_LIBRARY:bool=false \ --DGTSAM_BUILD_TESTS:bool=false \ --DGTSAM_BUILD_EXAMPLES:bool=false \ --DGTSAM_BUILD_UNSTABLE:bool=false \ --DGTSAM_DISABLE_EXAMPLES_ON_INSTALL:bool=true \ --DGTSAM_DISABLE_TESTS_ON_INSTALL:bool=true \ --DGTSAM_BUILD_CONVENIENCE_LIBRARIES:bool=false \ --DGTSAM_MEX_BUILD_STATIC_MODULE:bool=true .. +-DGTSAM_BUILD_TESTS:BOOL=OFF \ +-DGTSAM_BUILD_TIMING:BOOL=OFF \ +-DGTSAM_BUILD_EXAMPLES_ALWAYS:BOOL=OFF \ +-DGTSAM_WITH_TBB:BOOL=OFF \ +-DGTSAM_BUILD_METIS:BOOL=OFF \ +-DGTSAM_INSTALL_GEOGRAPHICLIB:BOOL=OFF \ +-DGTSAM_BUILD_UNSTABLE:BOOL=OFF \ +-DGTSAM_MEX_BUILD_STATIC_MODULE:BOOL=ON .. -if [ ! $? ]; then +if [ $? -ne 0 ]; then echo "CMake failed" exit 1 fi @@ -56,5 +55,10 @@ fi # Compile make -j8 install +if [ $? -ne 0 ]; then + echo "Compile failed" + exit 1 +fi + # Create package tar czf gtsam-toolbox-3.0.0-$platform.tgz -C stage/gtsam_toolbox toolbox diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 842f2bd67..a1d2d8c41 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -18,7 +18,10 @@ #include #include +#include +#include #include +#include #include #include @@ -105,24 +108,38 @@ TEST( Graph, composePoses ) CHECK(assert_equal(expected, *actual)); } -// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) -//{ -// GaussianFactorGraph g; -// Matrix I = eye(2); -// Vector b = Vector_(0, 0, 0); -// g += X(1), I, X(2), I, b, model; -// g += X(1), I, X(3), I, b, model; -// g += X(1), I, X(4), I, b, model; -// g += X(2), I, X(3), I, b, model; -// g += X(2), I, X(4), I, b, model; -// g += X(3), I, X(4), I, b, model; -// -// map tree = g.findMinimumSpanningTree(); -// EXPECT(tree[X(1)].compare(X(1))==0); -// EXPECT(tree[X(2)].compare(X(1))==0); -// EXPECT(tree[X(3)].compare(X(1))==0); -// EXPECT(tree[X(4)].compare(X(1))==0); -//} +/* ************************************************************************* */ +TEST( GaussianFactorGraph, findMinimumSpanningTree ) +{ + GaussianFactorGraph g; + Matrix I = eye(2); + Vector2 b(0, 0); + const SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 0.5, 0.5)); + using namespace symbol_shorthand; + g += JacobianFactor(X(1), I, X(2), I, b, model); + g += JacobianFactor(X(1), I, X(3), I, b, model); + g += JacobianFactor(X(1), I, X(4), I, b, model); + g += JacobianFactor(X(2), I, X(3), I, b, model); + g += JacobianFactor(X(2), I, X(4), I, b, model); + g += JacobianFactor(X(3), I, X(4), I, b, model); + + PredecessorMap tree = findMinimumSpanningTree(g); + EXPECT_LONGS_EQUAL(X(1),tree[X(1)]); + EXPECT_LONGS_EQUAL(X(1),tree[X(2)]); + EXPECT_LONGS_EQUAL(X(1),tree[X(3)]); + EXPECT_LONGS_EQUAL(X(1),tree[X(4)]); + + // we add a disconnected component - does not work yet + // g += JacobianFactor(X(5), I, X(6), I, b, model); + // + // PredecessorMap forest = findMinimumSpanningTree(g); + // EXPECT_LONGS_EQUAL(X(1),forest[X(1)]); + // EXPECT_LONGS_EQUAL(X(1),forest[X(2)]); + // EXPECT_LONGS_EQUAL(X(1),forest[X(3)]); + // EXPECT_LONGS_EQUAL(X(1),forest[X(4)]); + // EXPECT_LONGS_EQUAL(X(5),forest[X(5)]); + // EXPECT_LONGS_EQUAL(X(5),forest[X(6)]); +} ///* ************************************************************************* */ // SL-FIX TEST( GaussianFactorGraph, split ) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index b976cca90..6d8a056fc 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -295,7 +295,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { // test the diagonal GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); - GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear); + GaussianFactorGraph damped = *optimizer.buildDampedSystem(*linear); VectorValues d = linear->hessianDiagonal(), // expectedDiagonal = d + params.lambdaInitial * d; EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal())); @@ -309,7 +309,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { EXPECT(assert_equal(expectedGradient,linear->gradientAtZero())); // Check that the gradient is zero for damped system (it is not!) - damped = optimizer.buildDampedSystem(*linear); + damped = *optimizer.buildDampedSystem(*linear); VectorValues actualGradient = damped.gradientAtZero(); EXPECT(assert_equal(expectedGradient,actualGradient)); diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index c3798e5ce..d76556e4a 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -16,13 +16,14 @@ * @author Richard Roberts **/ -#include -#include -#include +#include "Argument.h" + #include #include -#include "Argument.h" +#include +#include +#include using namespace std; using namespace wrap; @@ -31,18 +32,24 @@ using namespace wrap; string Argument::matlabClass(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces) - result += ns + delim; - if (type=="string" || type=="unsigned char" || type=="char") + result += ns + delim; + if (type == "string" || type == "unsigned char" || type == "char") return result + "char"; - if (type=="Vector" || type=="Matrix") + if (type == "Vector" || type == "Matrix") return result + "double"; - if (type=="int" || type=="size_t") + if (type == "int" || type == "size_t") return result + "numeric"; - if (type=="bool") + if (type == "bool") return result + "logical"; return result + type; } +/* ************************************************************************* */ +bool Argument::isScalar() const { + return (type == "bool" || type == "char" || type == "unsigned char" + || type == "int" || type == "size_t" || type == "double"); +} + /* ************************************************************************* */ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << " "; @@ -52,7 +59,8 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer - file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; + file.oss << "boost::shared_ptr<" << cppType << "> " << name + << " = unwrap_shared_ptr< "; else if (is_ref) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; @@ -65,23 +73,28 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabUniqueType << "\""; + if (is_ptr || is_ref) + file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } /* ************************************************************************* */ string Argument::qualifiedType(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; + BOOST_FOREACH(const string& ns, namespaces) + result += ns + delim; return result + type; } /* ************************************************************************* */ string ArgumentList::types() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.type; first=false; + if (!first) + str += ","; + str += arg.type; + first = false; } return str; } @@ -89,16 +102,17 @@ string ArgumentList::types() const { /* ************************************************************************* */ string ArgumentList::signature() const { string sig; - bool cap=false; + bool cap = false; BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(char ch, arg.type) - if(isupper(ch)) { - sig += ch; - //If there is a capital letter, we don't want to read it below - cap=true; - } - if(!cap) sig += arg.type[0]; + if (isupper(ch)) { + sig += ch; + //If there is a capital letter, we don't want to read it below + cap = true; + } + if (!cap) + sig += arg.type[0]; //Reset to default cap = false; } @@ -109,23 +123,77 @@ string ArgumentList::signature() const { /* ************************************************************************* */ string ArgumentList::names() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.name; first=false; + if (!first) + str += ","; + str += arg.name; + first = false; } return str; } +/* ************************************************************************* */ +bool ArgumentList::allScalar() const { + BOOST_FOREACH(Argument arg, *this) + if (!arg.isScalar()) return false; + return true; +} + /* ************************************************************************* */ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { int index = start; BOOST_FOREACH(Argument arg, *this) { stringstream buf; buf << "in[" << index << "]"; - arg.matlab_unwrap(file,buf.str()); + arg.matlab_unwrap(file, buf.str()); index++; } } /* ************************************************************************* */ +void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { + file.oss << name << "("; + bool first = true; + BOOST_FOREACH(Argument arg, *this) { + if (!first) + file.oss << ", "; + file.oss << arg.type << " " << arg.name; + first = false; + } + file.oss << ")"; +} +/* ************************************************************************* */ +void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal, + const string& wrapperName, int id, bool staticMethod) const { + returnVal.emit_matlab(file); + file.oss << wrapperName << "(" << id; + if (!staticMethod) + file.oss << ", this"; + file.oss << ", varargin{:});\n"; +} +/* ************************************************************************* */ +void ArgumentList::emit_conditional_call(FileWriter& file, + const ReturnValue& returnVal, const string& wrapperName, int id, + bool staticMethod) const { + // Check nr of arguments + file.oss << "if length(varargin) == " << size(); + if (size() > 0) + file.oss << " && "; + // ...and their types + bool first = true; + for (size_t i = 0; i < size(); i++) { + if (!first) + file.oss << " && "; + file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".") + << "')"; + first = false; + } + file.oss << "\n"; + + // output call to C++ wrapper + file.oss << " "; + emit_call(file, returnVal, wrapperName, id, staticMethod); +} +/* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index f46eaa427..6f791978a 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -19,11 +19,12 @@ #pragma once +#include "FileWriter.h" +#include "ReturnValue.h" + #include #include -#include "FileWriter.h" - namespace wrap { /// Argument class @@ -40,6 +41,9 @@ struct Argument { /// return MATLAB class for use in isa(x,class) std::string matlabClass(const std::string& delim = "") const; + /// Check if will be unwrapped using scalar login in wrap/matlab.h + bool isScalar() const; + /// adds namespaces to type std::string qualifiedType(const std::string& delim = "") const; @@ -59,6 +63,9 @@ struct ArgumentList: public std::vector { /// create a comma-separated string listing all argument names, used in m-files std::string names() const; + /// Check if all arguments scalar + bool allScalar() const; + // MATLAB code generation: /** @@ -68,6 +75,32 @@ struct ArgumentList: public std::vector { */ void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++ + /** + * emit MATLAB prototype + * @param file output stream + * @param name of method or function + */ + void emit_prototype(FileWriter& file, const std::string& name) const; + + /** + * emit emit MATLAB call to wrapper + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call + */ + void emit_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod = false) const; + + /** + * emit conditional MATLAB call to wrapper (checking arguments first) + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call + */ + void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod = false) const; }; } // \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 3b8c41041..075c98811 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -7,62 +7,62 @@ * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - + * -------------------------------------------------------------------------- */ + /** * @file Class.cpp * @author Frank Dellaert * @author Andrew Melim * @author Richard Roberts - **/ - + **/ + +#include "Class.h" +#include "utilities.h" +#include "Argument.h" + +#include +#include + #include #include #include //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC - -#include -#include - -#include "Class.h" -#include "utilities.h" -#include "Argument.h" - -using namespace std; -using namespace wrap; - -/* ************************************************************************* */ -void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, - const TypeAttributesTable& typeAttributes, - FileWriter& wrapperFile, vector& functionNames) const { - +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, + const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, + vector& functionNames) const { + // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); - + createNamespaceStructure(namespaces, toolboxPath); + // open destination classFile - string classFile = toolboxPath; - if(!namespaces.empty()) - classFile += "/+" + wrap::qualifiedName("/+", namespaces); - classFile += "/" + name + ".m"; - FileWriter proxyFile(classFile, verbose_, "%"); - + string classFile = toolboxPath; + if (!namespaces.empty()) + classFile += "/+" + wrap::qualifiedName("/+", namespaces); + classFile += "/" + name + ".m"; + FileWriter proxyFile(classFile, verbose_, "%"); + // get the name of actual matlab object - const string matlabQualName = qualifiedName("."), matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); - const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); - + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppName = qualifiedName("::"); + const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); + const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); + // emit class proxy code // we want our class to inherit the handle class for memory purposes - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; - + // Constructor proxyFile.oss << " function obj = " << name << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and @@ -72,284 +72,325 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, // other wrap modules - to add these to their collectors the pointer is // passed from one C++ module into matlab then back into the other C++ // module. - pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - wrapperFile.oss << "\n"; + pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); + wrapperFile.oss << "\n"; // Regular constructors - BOOST_FOREACH(ArgumentList a, constructor.args_list) - { - const int id = (int)functionNames.size(); - constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), id, a); - const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, - cppName, matlabUniqueName, cppBaseName, id, a); - wrapperFile.oss << "\n"; - functionNames.push_back(wrapFunctionName); - } - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; - proxyFile.oss << " end\n"; - if(!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; - proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; - proxyFile.oss << " end\n\n"; - + BOOST_FOREACH(ArgumentList a, constructor.args_list) { + const int id = (int) functionNames.size(); + constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), + id, a); + const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, cppBaseName, id, a); + wrapperFile.oss << "\n"; + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss << " error('Arguments do not match any overload of " + << matlabQualName << " constructor');\n"; + proxyFile.oss << " end\n"; + if (!qualifiedParent.empty()) + proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" + << ptr_constructor_key << "), base_ptr);\n"; + proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; + proxyFile.oss << " end\n\n"; + // Deconstructor - { - const int id = (int)functionNames.size(); - deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); - proxyFile.oss << "\n"; - const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, id); - wrapperFile.oss << "\n"; - functionNames.push_back(functionName); - } - proxyFile.oss << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; - proxyFile.oss << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; - + { + const int id = (int) functionNames.size(); + deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); + proxyFile.oss << "\n"; + const string functionName = deconstructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, id); + wrapperFile.oss << "\n"; + functionNames.push_back(functionName); + } + proxyFile.oss + << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; + proxyFile.oss + << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; + // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { - const Method& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const Methods::value_type& name_m, methods) { + const Method& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - - proxyFile.oss << " end\n"; - proxyFile.oss << "\n"; - proxyFile.oss << " methods(Static = true)\n"; - + + proxyFile.oss << " end\n"; + proxyFile.oss << "\n"; + proxyFile.oss << " methods(Static = true)\n"; + // Static methods - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { - const StaticMethod& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { + const StaticMethod& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) - deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); + deserialization_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); proxyFile.oss << " end\n"; proxyFile.oss << "end\n"; - + // Close file - proxyFile.emit(true); -} - -/* ************************************************************************* */ -string Class::qualifiedName(const string& delim) const { - return ::wrap::qualifiedName(delim, namespaces, name); -} - -/* ************************************************************************* */ -void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector& functionNames) const { - - const string matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string baseCppName = wrap::qualifiedName("::", qualifiedParent); - - const int collectorInsertId = (int)functionNames.size(); - const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast(collectorInsertId); - functionNames.push_back(collectorInsertFunctionName); - - int upcastFromVoidId; - string upcastFromVoidFunctionName; - if(isVirtual) { - upcastFromVoidId = (int)functionNames.size(); - upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast(upcastFromVoidId); - functionNames.push_back(upcastFromVoidFunctionName); - } - + proxyFile.emit(true); +} + +/* ************************************************************************* */ +string Class::qualifiedName(const string& delim) const { + return ::wrap::qualifiedName(delim, namespaces, name); +} + +/* ************************************************************************* */ +void Class::pointer_constructor_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& wrapperName, + vector& functionNames) const { + + const string matlabUniqueName = qualifiedName(), cppName = qualifiedName( + "::"); + const string baseCppName = wrap::qualifiedName("::", qualifiedParent); + + const int collectorInsertId = (int) functionNames.size(); + const string collectorInsertFunctionName = matlabUniqueName + + "_collectorInsertAndMakeBase_" + + boost::lexical_cast(collectorInsertId); + functionNames.push_back(collectorInsertFunctionName); + + int upcastFromVoidId; + string upcastFromVoidFunctionName; + if (isVirtual) { + upcastFromVoidId = (int) functionNames.size(); + upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + + boost::lexical_cast(upcastFromVoidId); + functionNames.push_back(upcastFromVoidFunctionName); + } + // MATLAB constructor that assigns pointer to matlab object then calls c++ // function to add the object to the collector. - if(isVirtual) { - proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; - } else { - proxyFile.oss << " if nargin == 2"; - } - proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" << ptr_constructor_key << ")\n"; - if(isVirtual) { - proxyFile.oss << " if nargin == 2\n"; - proxyFile.oss << " my_ptr = varargin{2};\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " my_ptr = " << wrapperName << "(" << upcastFromVoidId << ", varargin{2});\n"; - proxyFile.oss << " end\n"; - } else { - proxyFile.oss << " my_ptr = varargin{2};\n"; - } - if(qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back - proxyFile.oss << " "; - else - proxyFile.oss << " base_ptr = "; + if (isVirtual) { + proxyFile.oss + << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; + } else { + proxyFile.oss << " if nargin == 2"; + } + proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" + << ptr_constructor_key << ")\n"; + if (isVirtual) { + proxyFile.oss << " if nargin == 2\n"; + proxyFile.oss << " my_ptr = varargin{2};\n"; + proxyFile.oss << " else\n"; + proxyFile.oss << " my_ptr = " << wrapperName << "(" + << upcastFromVoidId << ", varargin{2});\n"; + proxyFile.oss << " end\n"; + } else { + proxyFile.oss << " my_ptr = varargin{2};\n"; + } + if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + proxyFile.oss << " "; + else + proxyFile.oss << " base_ptr = "; proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr - + // C++ function to add pointer from MATLAB to collector. The pointer always // comes from a C++ return value; this mechanism allows the object to be added // to a collector in a different wrap module. If this class has a base class, // a new pointer to the base class is allocated and returned. - wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; + wrapperFile.oss << "void " << collectorInsertFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; // Typedef boost::shared_ptr - wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; - wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; + wrapperFile.oss << "\n"; // Get self pointer passed in - wrapperFile.oss << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; + wrapperFile.oss + << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; // Add to collector - wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; + wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if(!qualifiedParent.empty()) { - wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName << "> SharedBase;\n"; - wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; - wrapperFile.oss << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; - } - wrapperFile.oss << "}\n"; - + if (!qualifiedParent.empty()) { + wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + << "> SharedBase;\n"; + wrapperFile.oss + << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; + wrapperFile.oss + << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; + } + wrapperFile.oss << "}\n"; + // If this is a virtual function, C++ function to dynamic upcast it from a // shared_ptr. This mechanism allows automatic dynamic creation of the // real underlying derived-most class when a C++ method returns a virtual // base class. - if(isVirtual) - wrapperFile.oss << - "\n" - "void " << upcastFromVoidFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" - " mexAtExit(&_deleteAllObjects);\n" - " typedef boost::shared_ptr<" << cppName << "> Shared;\n" - " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" - " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" - " Shared *self = new Shared(boost::static_pointer_cast<" << cppName << ">(*asVoid));\n" - " *reinterpret_cast(mxGetData(out[0])) = self;\n" - "}\n"; -} - -/* ************************************************************************* */ -vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - vector result; - BOOST_FOREACH(const ArgumentList& argList, argLists) { - ArgumentList instArgList; - BOOST_FOREACH(const Argument& arg, argList) { - Argument instArg = arg; - if(arg.type == templateArg) { - instArg.namespaces.assign(instName.begin(), instName.end()-1); - instArg.type = instName.back(); - } else if(arg.type == "This") { - instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instArg.type = expandedClassName; - } - instArgList.push_back(instArg); - } - result.push_back(instArgList); - } - return result; -} - -/* ************************************************************************* */ -template -map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - map result; - typedef pair Name_Method; - BOOST_FOREACH(const Name_Method& name_method, methods) { - const METHOD& method = name_method.second; - METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName, expandedClassNamespace, expandedClassName); - instMethod.returnVals.clear(); - BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { - ReturnValue instRetVal = retVal; - if(retVal.type1 == templateArg) { - instRetVal.namespaces1.assign(instName.begin(), instName.end()-1); - instRetVal.type1 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces1.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type1 = expandedClassName; - } - if(retVal.type2 == templateArg) { - instRetVal.namespaces2.assign(instName.begin(), instName.end()-1); - instRetVal.type2 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces2.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type2 = expandedClassName; - } - instMethod.returnVals.push_back(instRetVal); - } - result.insert(make_pair(name_method.first, instMethod)); - } - return result; -} - -/* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - Class inst; - inst.name = cls.name; - inst.templateArgs = cls.templateArgs; - inst.typedefName = cls.typedefName; - inst.isVirtual = cls.isVirtual; + if (isVirtual) + wrapperFile.oss << "\n" + "void " << upcastFromVoidFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" + " mexAtExit(&_deleteAllObjects);\n" + " typedef boost::shared_ptr<" << cppName + << "> Shared;\n" + " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" + " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" + " Shared *self = new Shared(boost::static_pointer_cast<" << cppName + << ">(*asVoid));\n" + " *reinterpret_cast(mxGetData(out[0])) = self;\n" + "}\n"; +} + +/* ************************************************************************* */ +vector expandArgumentListsTemplate( + const vector& argLists, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + vector result; + BOOST_FOREACH(const ArgumentList& argList, argLists) { + ArgumentList instArgList; + BOOST_FOREACH(const Argument& arg, argList) { + Argument instArg = arg; + if (arg.type == templateArg) { + instArg.namespaces.assign(instName.begin(), instName.end() - 1); + instArg.type = instName.back(); + } else if (arg.type == "This") { + instArg.namespaces.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instArg.type = expandedClassName; + } + instArgList.push_back(instArg); + } + result.push_back(instArgList); + } + return result; +} + +/* ************************************************************************* */ +template +map expandMethodTemplate(const map& methods, + const string& templateArg, const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + map result; + typedef pair Name_Method; + BOOST_FOREACH(const Name_Method& name_method, methods) { + const METHOD& method = name_method.second; + METHOD instMethod = method; + instMethod.argLists = expandArgumentListsTemplate(method.argLists, + templateArg, instName, expandedClassNamespace, expandedClassName); + instMethod.returnVals.clear(); + BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { + ReturnValue instRetVal = retVal; + if (retVal.type1 == templateArg) { + instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1); + instRetVal.type1 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces1.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type1 = expandedClassName; + } + if (retVal.type2 == templateArg) { + instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1); + instRetVal.type2 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces2.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type2 = expandedClassName; + } + instMethod.returnVals.push_back(instRetVal); + } + result.insert(make_pair(name_method.first, instMethod)); + } + return result; +} + +/* ************************************************************************* */ +Class expandClassTemplate(const Class& cls, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + Class inst; + inst.name = cls.name; + inst.templateArgs = cls.templateArgs; + inst.typedefName = cls.typedefName; + inst.isVirtual = cls.isVirtual; inst.isSerializable = cls.isSerializable; - inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.namespaces = cls.namespaces; - inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.constructor.name = inst.name; - inst.deconstructor = cls.deconstructor; - inst.deconstructor.name = inst.name; - inst.verbose_ = cls.verbose_; - return inst; -} - -/* ************************************************************************* */ -vector Class::expandTemplate(const string& templateArg, const vector >& instantiations) const { - vector result; - BOOST_FOREACH(const vector& instName, instantiations) { - const string expandedName = name + instName.back(); - Class inst = expandClassTemplate(*this, templateArg, instName, this->namespaces, expandedName); - inst.name = expandedName; - inst.templateArgs.clear(); - inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">"; - result.push_back(inst); - } - return result; -} - -/* ************************************************************************* */ -Class Class::expandTemplate(const string& templateArg, const vector& instantiation, const std::vector& expandedClassNamespace, const string& expandedClassName) const { - return expandClassTemplate(*this, templateArg, instantiation, expandedClassNamespace, expandedClassName); -} - -/* ************************************************************************* */ -std::string Class::getTypedef() const { - string result; - BOOST_FOREACH(const string& namesp, namespaces) { - result += ("namespace " + namesp + " { "); - } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i Class::expandTemplate(const string& templateArg, + const vector >& instantiations) const { + vector result; + BOOST_FOREACH(const vector& instName, instantiations) { + const string expandedName = name + instName.back(); + Class inst = expandClassTemplate(*this, templateArg, instName, + this->namespaces, expandedName); + inst.name = expandedName; + inst.templateArgs.clear(); + inst.typedefName = qualifiedName("::") + "<" + + wrap::qualifiedName("::", instName) + ">"; + result.push_back(inst); + } + return result; +} + +/* ************************************************************************* */ +Class Class::expandTemplate(const string& templateArg, + const vector& instantiation, + const std::vector& expandedClassNamespace, + const string& expandedClassName) const { + return expandClassTemplate(*this, templateArg, instantiation, + expandedClassNamespace, expandedClassName); +} + +/* ************************************************************************* */ +std::string Class::getTypedef() const { + string result; + BOOST_FOREACH(const string& namesp, namespaces) { + result += ("namespace " + namesp + " { "); + } + result += ("typedef " + typedefName + " " + name + ";"); + for (size_t i = 0; i < namespaces.size(); ++i) { + result += " }"; + } + return result; +} + +/* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name - << ", see Doxygen page for details\n"; + proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; if (!constructor.args_list.empty()) proxyFile.oss << "%\n%-------Constructors-------\n"; BOOST_FOREACH(ArgumentList argList, constructor.args_list) { - string up_name = boost::to_upper_copy(name); - proxyFile.oss << "%" << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")\n"; + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << "\n"; } if (!methods.empty()) @@ -357,17 +398,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const Methods::value_type& name_m, methods) { const Method& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } @@ -377,18 +410,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { const StaticMethod& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } @@ -396,15 +420,17 @@ void Class::comment_fragment(FileWriter& proxyFile) const { if (hasSerialization) { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; - proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n"; + proxyFile.oss << "%string_deserialize(string serialized) : returns " + << this->name << "\n"; } proxyFile.oss << "%\n"; } -/* ************************************************************************* */ -void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +/* ************************************************************************* */ +void Class::serialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ @@ -418,30 +444,34 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi //} int serialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast(serialize_id); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameSerialize = matlabUniqueName + + "_string_serialize_" + boost::lexical_cast(serialize_id); functionNames.push_back(wrapFunctionNameSerialize); // call //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) - wrapperFile.oss << "void " << wrapFunctionNameSerialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "void " << wrapFunctionNameSerialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; // check arguments - for serialize, no arguments // example: checkArguments("string_serialize",nargout,nargin-1,0); - wrapperFile.oss << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; + wrapperFile.oss + << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; // get class pointer // example: Shared obj = unwrap_shared_ptr(in[0], "ptr_Point3"); - wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // Serialization boilerplate wrapperFile.oss << " std::ostringstream out_archive_stream;\n"; - wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; + wrapperFile.oss + << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; wrapperFile.oss << " out_archive << *obj;\n"; wrapperFile.oss << " out[0] = wrap< string >(out_archive_stream.str());\n"; @@ -459,13 +489,19 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end // end - proxyFile.oss << " function varargout = string_serialize(this, varargin)\n"; - proxyFile.oss << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss + << " function varargout = string_serialize(this, varargin)\n"; + proxyFile.oss + << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; proxyFile.oss << " if length(varargin) == 0\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_serialize');\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_serialize');\n"; proxyFile.oss << " end\n"; proxyFile.oss << " end\n\n"; @@ -476,14 +512,16 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end proxyFile.oss << " function sobj = saveobj(obj)\n"; - proxyFile.oss << " % SAVEOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss + << " % SAVEOBJ Saves the object to a matlab-readable format\n"; proxyFile.oss << " sobj = obj.string_serialize();\n"; proxyFile.oss << " end\n"; } /* ************************************************************************* */ -void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +void Class::deserialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; @@ -495,32 +533,36 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} - int deserialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast(deserialize_id); - functionNames.push_back(wrapFunctionNameDeserialize); + int deserialize_id = functionNames.size(); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameDeserialize = matlabUniqueName + + "_string_deserialize_" + boost::lexical_cast(deserialize_id); + functionNames.push_back(wrapFunctionNameDeserialize); - // call - wrapperFile.oss << "void " << wrapFunctionNameDeserialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + // call + wrapperFile.oss << "void " << wrapFunctionNameDeserialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; - // check arguments - for deserialize, 1 string argument - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << ".string_deserialize\",nargout,nargin,1);\n"; + // check arguments - for deserialize, 1 string argument + wrapperFile.oss << " checkArguments(\"" << matlabUniqueName + << ".string_deserialize\",nargout,nargin,1);\n"; - // string argument with deserialization boilerplate - wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; - wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; - wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; - wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; - wrapperFile.oss << " in_archive >> *output;\n"; - wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName << "\", false);\n"; - wrapperFile.oss << "}\n"; + // string argument with deserialization boilerplate + wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; + wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; + wrapperFile.oss + << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; + wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; + wrapperFile.oss << " in_archive >> *output;\n"; + wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName + << "\", false);\n"; + wrapperFile.oss << "}\n"; - // Generate matlab function + // Generate matlab function // function varargout = string_deserialize(varargin) // % STRING_DESERIALIZE usage: string_deserialize() : returns Point3 // % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html @@ -531,32 +573,40 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // end // end - proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; - proxyFile.oss << " % STRING_DESERIALIZE usage: string_deserialize() : returns " << matlabQualName << "\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; - proxyFile.oss << " if length(varargin) == 1\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_deserialize');\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n\n"; + proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; + proxyFile.oss + << " % STRING_DESERIALIZE usage: string_deserialize() : returns " + << matlabQualName << "\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss << " if length(varargin) == 1\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_deserialize');\n"; + proxyFile.oss << " end\n"; + proxyFile.oss << " end\n\n"; - - // Generate matlab load function + // Generate matlab load function // function obj = loadobj(sobj) // % LOADOBJ Saves the object to a matlab-readable format // obj = Point3.string_deserialize(sobj); // end - proxyFile.oss << " function obj = loadobj(sobj)\n"; - proxyFile.oss << " % LOADOBJ Saves the object to a matlab-readable format\n"; - proxyFile.oss << " obj = " << matlabQualName << ".string_deserialize(sobj);\n"; - proxyFile.oss << " end" << endl; + proxyFile.oss << " function obj = loadobj(sobj)\n"; + proxyFile.oss + << " % LOADOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss << " obj = " << matlabQualName + << ".string_deserialize(sobj);\n"; + proxyFile.oss << " end" << endl; } /* ************************************************************************* */ std::string Class::getSerializationExport() const { //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal"); - return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");"; + return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + + qualifiedName() + "\");"; } /* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index f5267cee2..2ca976f66 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -27,7 +27,6 @@ #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" -#include namespace wrap { diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 96d1030a1..5438c515c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,11 +18,11 @@ #pragma once +#include "Argument.h" + #include #include -#include "Argument.h" - namespace wrap { // Constructor class @@ -34,7 +34,7 @@ struct Constructor { } // Then the instance variables are set directly by the Module constructor - std::vector args_list; + std::vector args_list; std::string name; bool verbose_; @@ -50,21 +50,18 @@ struct Constructor { * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end */ void proxy_fragment(FileWriter& file, const std::string& wrapperName, - bool hasParent, const int id, const ArgumentList args) const; + bool hasParent, const int id, const ArgumentList args) const; /// cpp wrapper std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - const std::string& cppBaseClassName, - int id, - const ArgumentList& al) const; + const std::string& cppClassName, const std::string& matlabUniqueName, + const std::string& cppBaseClassName, int id, + const ArgumentList& al) const; /// constructor function void generate_construct(FileWriter& file, const std::string& cppClassName, - std::vector& args_list) const; - + std::vector& args_list) const; + }; - } // \namespace wrap diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index c5e64614e..3f5461078 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -28,7 +28,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { bool file_exists = true; try { existing_contents = file_contents(filename_.c_str(), add_header); - } catch (CantOpenFile& e) { + } catch (CantOpenFile) { file_exists = false; } diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 16f6d48f1..25e1dcedb 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -17,7 +17,8 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack) { + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack) { this->verbose_ = verbose; this->name = name; this->argLists.push_back(args); @@ -26,16 +27,16 @@ void GlobalFunction::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void GlobalFunction::matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const { +void GlobalFunction::matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // cluster overloads with same namespace // create new GlobalFunction structures around namespaces - same namespaces and names are overloads // map of namespace to global function typedef map GlobalFunctionMap; GlobalFunctionMap grouped_functions; - for (size_t i=0; i& functionNames) const { +void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // create the folder for the namespace const StrVec& ns = namespaces.front(); @@ -68,84 +70,68 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons // open destination mfunctionFileName string mfunctionFileName = toolboxPath; - if(!ns.empty()) + if (!ns.empty()) mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns); mfunctionFileName += "/" + name + ".m"; FileWriter mfunctionFile(mfunctionFileName, verbose_, "%"); // get the name of actual matlab object - const string - matlabQualName = qualifiedName(".", ns, name), - matlabUniqueName = qualifiedName("", ns, name), - cppName = qualifiedName("::", ns, name); + const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName = + qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name); mfunctionFile.oss << "function varargout = " << name << "(varargin)\n"; - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); const int id = functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - mfunctionFile.oss << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) mfunctionFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + + boost::lexical_cast(id); // call - wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start - wrapperFile.oss << "{\n"; + file.oss << "{\n"; - returnVal.wrapTypeUnwrap(wrapperFile); + returnVal.wrapTypeUnwrap(file); // check arguments // NOTE: for static functions, there is no object passed - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(wrapperFile,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppName+"("+args.names()+")", wrapperFile, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppName + "(" + args.names() + ")", file, + typeAttributes); else - wrapperFile.oss << cppName+"("+args.names()+");\n"; + file.oss << cppName + "(" + args.names() + ");\n"; // finish - wrapperFile.oss << "}\n"; + file.oss << "}\n"; // Add to function list functionNames.push_back(wrapFunctionName); } - mfunctionFile.oss << "else\n"; - mfunctionFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << "');" << endl; - mfunctionFile.oss << "end" << endl; + mfunctionFile.oss << " else\n"; + mfunctionFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "');" << endl; + mfunctionFile.oss << " end" << endl; // Close file mfunctionFile.emit(true); @@ -153,9 +139,5 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons /* ************************************************************************* */ - } // \namespace wrap - - - diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 2ecaf4998..6c8ad0c86 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -22,37 +22,38 @@ struct GlobalFunction { std::string name; // each overload, regardless of namespace - std::vector argLists; ///< arugments for each overload - std::vector returnVals; ///< returnVals for each overload - std::vector namespaces; ///< Stack of namespaces + std::vector argLists; ///< arugments for each overload + std::vector returnVals; ///< returnVals for each overload + std::vector namespaces; ///< Stack of namespaces // Constructor only used in Module - GlobalFunction(bool verbose = true) : verbose_(verbose) {} + GlobalFunction(bool verbose = true) : + verbose_(verbose) { + } // Used to reconstruct - GlobalFunction(const std::string& name_, bool verbose = true) - : verbose_(verbose), name(name_) {} + GlobalFunction(const std::string& name_, bool verbose = true) : + verbose_(verbose), name(name_) { + } // adds an overloaded version of this function void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack); + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack); // codegen function called from Module to build the cpp and matlab versions of the function - void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; private: // Creates a single global function - all in same namespace - void generateSingleFunction(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; }; } // \namespace wrap - - - diff --git a/wrap/Method.cpp b/wrap/Method.cpp index b327ac7dc..7b88b9cdc 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -15,14 +15,15 @@ * @author Richard Roberts **/ -#include -#include +#include "Method.h" +#include "utilities.h" #include #include +#include -#include "Method.h" -#include "utilities.h" +#include +#include using namespace std; using namespace wrap; @@ -38,155 +39,140 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name, } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, + const string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const string& wrapperName, + const TypeAttributesTable& typeAttributes, + vector& functionNames) const { - proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; - //Comments for documentation - string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + // Create function header + file.oss << " function varargout = " << name << "(this, varargin)\n"; + + // Emit comments for documentation + string up_name = boost::to_upper_copy(name); + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } - - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; - } - - for(size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = functionNames.size(); - - // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i 1) { + file.oss << " % " << "" << endl; + file.oss << " % " << "Method Overloads" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + file.oss << " % "; + argList.emit_prototype(file, name); + file.oss << endl; + } + } + + // Handle special case of single overload with all numeric arguments + if (argLists.size() == 1 && argLists[0].allScalar()) { + // Output proxy matlab code + file.oss << " "; + const int id = (int) functionNames.size(); + argLists[0].emit_call(file, returnVals[0], wrapperName, id); // Output C++ wrapper code - - const string wrapFunctionName = wrapper_fragment( - wrapperFile, cppClassName, matlabUniqueName, overload, id, typeAttributes); + const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, + matlabUniqueName, 0, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); + } else { + // Check arguments for all overloads + for (size_t overload = 0; overload < argLists.size(); ++overload) { + // Output proxy matlab code + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, overload, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + file.oss << " else\n"; + file.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name << "');" << endl; + file.oss << " end\n"; } - - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << - matlabQualName << "." << name << "');" << endl; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n"; + file.oss << " end\n"; } /* ************************************************************************* */ -string Method::wrapper_fragment(FileWriter& file, - const string& cppClassName, - const string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const { +string Method::wrapper_fragment(FileWriter& file, const string& cppClassName, + const string& matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const { // generate code - const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; - if(returnVal.isPair) - { - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; - if(returnVal.category2 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl; - } - else - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + if (returnVal.isPair) { + if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 + << ";" << endl; + if (returnVal.category2 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 + << ";" << endl; + } else if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") + << "> Shared" << returnVal.type1 << ";" << endl; - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," + << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,1); + args.matlab_unwrap(file, 1); // call method and wrap result // example: out[0]=wrap(self->return_field(t)); - if (returnVal.type1!="void") - returnVal.wrap_result("obj->"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file, + typeAttributes); else - file.oss << " obj->"+name+"("+args.names()+");\n"; + file.oss << " obj->" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; diff --git a/wrap/Method.h b/wrap/Method.h index f2a7ed190..9926a5179 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,13 +18,12 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include + +#include +#include namespace wrap { diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 6870d5178..2cb5ea7ed 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -98,7 +98,7 @@ void Module::parseMarkup(const std::string& data) { // The one with postfix 0 are used to reset the variables after parse. string methodName, methodName0; bool isConst, isConst0 = false; - ReturnValue retVal0(verbose), retVal(verbose); + ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; vector arg_dup; ///keep track of duplicates diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 3cb68181b..78e36d4da 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -141,5 +141,13 @@ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { } } /* ************************************************************************* */ +void ReturnValue::emit_matlab(FileWriter& file) const { + string output; + if (isPair) + file.oss << "[ varargout{1} varargout{2} ] = "; + else if (category1 != ReturnValue::VOID) + file.oss << "varargout{1} = "; +} +/* ************************************************************************* */ diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 7a677532f..989f81109 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -8,36 +8,36 @@ * @author Richard Roberts */ -#include -#include - #include "FileWriter.h" #include "TypeAttributesTable.h" +#include + #pragma once namespace wrap { +/** + * Encapsulates return value of a method or function + */ struct ReturnValue { + /// the different supported return value categories typedef enum { - CLASS = 1, - EIGEN = 2, - BASIS = 3, - VOID = 4 + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - ReturnValue(bool enable_verbosity = true) - : verbose(enable_verbosity), isPtr1(false), isPtr2(false), - isPair(false), category1(CLASS), category2(CLASS) - {} - - bool verbose; bool isPtr1, isPtr2, isPair; return_category category1, category2; std::string type1, type2; std::vector namespaces1, namespaces2; + /// Constructor + ReturnValue() : + isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2( + CLASS) { + } + typedef enum { arg1, arg2, pair } pairing; @@ -49,10 +49,12 @@ struct ReturnValue { std::string matlab_returnType() const; - void wrap_result(const std::string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const; + void wrap_result(const std::string& result, FileWriter& file, + const TypeAttributesTable& typeAttributes) const; void wrapTypeUnwrap(FileWriter& wrapperFile) const; + void emit_matlab(FileWriter& file) const; }; } // \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index e5c10b4c8..0c4cc7d75 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,19 +16,19 @@ * @author Richard Roberts **/ -#include -#include - -#include -#include - #include "StaticMethod.h" #include "utilities.h" +#include +#include +#include + +#include +#include + using namespace std; using namespace wrap; - /* ************************************************************************* */ void StaticMethod::addOverload(bool verbose, const std::string& name, const ArgumentList& args, const ReturnValue& retVal) { @@ -39,144 +39,103 @@ void StaticMethod::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void StaticMethod::proxy_wrapper_fragments(FileWriter& file, + FileWriter& wrapperFile, const string& cppClassName, + const std::string& matlabQualName, const std::string& matlabUniqueName, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + vector& functionNames) const { - string upperName = name; upperName[0] = std::toupper(upperName[0], std::locale()); + string upperName = name; + upperName[0] = std::toupper(upperName[0], std::locale()); - proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; + file.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation - string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + string up_name = boost::to_upper_copy(name); + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } + BOOST_FOREACH(ArgumentList argList, argLists) { + argList.emit_prototype(file, name); + if (argLCount != argLists.size() - 1) + file.oss << ", "; + else + file.oss << " : returns " + << returnVals[0].return_type(false, returnVals[0].pair) << endl; + argLCount++; + } - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Usage" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << up_name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; - } + file.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; + file.oss << " % " << "" << endl; + file.oss << " % " << "Usage" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + file.oss << " % "; + argList.emit_prototype(file, up_name); + file.oss << endl; + } - - for(size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = (int)functionNames.size(); + // Check arguments for all overloads + for (size_t overload = 0; overload < argLists.size(); ++overload) { // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; returnVal.wrapTypeUnwrap(file); - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // NOTE: for static functions, there is no object passed - file.oss << " checkArguments(\"" << matlabUniqueName << "." << name << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName << "." << name + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")", + file, typeAttributes); else - file.oss << cppClassName+"::"+name+"("+args.names()+");\n"; + file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 3e8dc08cf..e1855f4c2 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,13 +19,9 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include namespace wrap { @@ -34,7 +30,8 @@ struct StaticMethod { /// Constructor creates empty object StaticMethod(bool verbosity = true) : - verbose(verbosity) {} + verbose(verbosity) { + } // Then the instance variables are set directly by the Module constructor bool verbose; @@ -46,22 +43,20 @@ struct StaticMethod { // with those in rhs, but in subsequent calls it adds additional argument // lists as function overloads. void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); + const ArgumentList& args, const ReturnValue& retVal); // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; + const std::string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const std::string& wrapperName, + const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; private: std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + const std::string& cppClassName, const std::string& matlabUniqueName, + int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper }; } // \namespace wrap diff --git a/wrap/tests/expected/.gitignore b/wrap/tests/expected/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/wrap/tests/expected/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index 22dec9641..9f64f2d10 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -44,92 +44,43 @@ classdef Point2 < handle function varargout = argChar(this, varargin) % ARGCHAR usage: argChar(char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argChar(char a) - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(4, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argChar'); - end + geometry_wrapper(4, this, varargin{:}); end function varargout = argUChar(this, varargin) % ARGUCHAR usage: argUChar(unsigned char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argUChar(unsigned char a) - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(5, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argUChar'); - end + geometry_wrapper(5, this, varargin{:}); end function varargout = dim(this, varargin) % DIM usage: dim() : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % dim() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(6, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.dim'); - end + varargout{1} = geometry_wrapper(6, this, varargin{:}); end function varargout = returnChar(this, varargin) % RETURNCHAR usage: returnChar() : returns char % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % returnChar() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(7, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.returnChar'); - end + varargout{1} = geometry_wrapper(7, this, varargin{:}); end function varargout = vectorConfusion(this, varargin) % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % vectorConfusion() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(8, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.vectorConfusion'); - end + varargout{1} = geometry_wrapper(8, this, varargin{:}); end function varargout = x(this, varargin) % X usage: x() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % x() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.x'); - end + varargout{1} = geometry_wrapper(9, this, varargin{:}); end function varargout = y(this, varargin) % Y usage: y() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % y() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(10, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.y'); - end + varargout{1} = geometry_wrapper(10, this, varargin{:}); end end diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index 94d9c25b8..8a43987b9 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -43,14 +43,7 @@ classdef Point3 < handle function varargout = norm(this, varargin) % NORM usage: norm() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % norm() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(14, this, varargin{:}); - else - error('Arguments do not match any overload of function Point3.norm'); - end + varargout{1} = geometry_wrapper(14, this, varargin{:}); end function varargout = string_serialize(this, varargin) diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index 8e56df6fc..1afd15efe 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -56,9 +56,6 @@ classdef Test < handle function varargout = arg_EigenConstRef(this, varargin) % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % arg_EigenConstRef(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') geometry_wrapper(23, this, varargin{:}); else @@ -69,61 +66,30 @@ classdef Test < handle function varargout = create_MixedPtrs(this, varargin) % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_MixedPtrs() - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_MixedPtrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); end function varargout = create_ptrs(this, varargin) % CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_ptrs() - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_ptrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); end function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % print() - if length(varargin) == 0 - geometry_wrapper(26, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.print'); - end + geometry_wrapper(26, this, varargin{:}); end function varargout = return_Point2Ptr(this, varargin) % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Point2Ptr(bool value) - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(27, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_Point2Ptr'); - end + varargout{1} = geometry_wrapper(27, this, varargin{:}); end function varargout = return_Test(this, varargin) % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Test(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(28, this, varargin{:}); else @@ -134,9 +100,6 @@ classdef Test < handle function varargout = return_TestPtr(this, varargin) % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_TestPtr(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(29, this, varargin{:}); else @@ -147,35 +110,18 @@ classdef Test < handle function varargout = return_bool(this, varargin) % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_bool(bool value) - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(30, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_bool'); - end + varargout{1} = geometry_wrapper(30, this, varargin{:}); end function varargout = return_double(this, varargin) % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_double(double value) - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = geometry_wrapper(31, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_double'); - end + varargout{1} = geometry_wrapper(31, this, varargin{:}); end function varargout = return_field(this, varargin) % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_field(Test t) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(32, this, varargin{:}); else @@ -186,22 +132,12 @@ classdef Test < handle function varargout = return_int(this, varargin) % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_int(int value) - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(33, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_int'); - end + varargout{1} = geometry_wrapper(33, this, varargin{:}); end function varargout = return_matrix1(this, varargin) % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix1(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(34, this, varargin{:}); else @@ -212,9 +148,6 @@ classdef Test < handle function varargout = return_matrix2(this, varargin) % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix2(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(35, this, varargin{:}); else @@ -225,9 +158,6 @@ classdef Test < handle function varargout = return_pair(this, varargin) % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_pair(Vector v, Matrix A) if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') [ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:}); else @@ -238,9 +168,6 @@ classdef Test < handle function varargout = return_ptrs(this, varargin) % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_ptrs(Test p1, Test p2) if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') [ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:}); else @@ -251,22 +178,12 @@ classdef Test < handle function varargout = return_size_t(this, varargin) % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_size_t(size_t value) - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(38, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_size_t'); - end + varargout{1} = geometry_wrapper(38, this, varargin{:}); end function varargout = return_string(this, varargin) % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_string(string value) if length(varargin) == 1 && isa(varargin{1},'char') varargout{1} = geometry_wrapper(39, this, varargin{:}); else @@ -277,9 +194,6 @@ classdef Test < handle function varargout = return_vector1(this, varargin) % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector1(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(40, this, varargin{:}); else @@ -290,9 +204,6 @@ classdef Test < handle function varargout = return_vector2(this, varargin) % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector2(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(41, this, varargin{:}); else diff --git a/wrap/tests/expected/aGlobalFunction.m b/wrap/tests/expected/aGlobalFunction.m index 09ece0b83..94e9b4a67 100644 --- a/wrap/tests/expected/aGlobalFunction.m +++ b/wrap/tests/expected/aGlobalFunction.m @@ -1,6 +1,6 @@ function varargout = aGlobalFunction(varargin) -if length(varargin) == 0 - varargout{1} = geometry_wrapper(42, varargin{:}); -else - error('Arguments do not match any overload of function aGlobalFunction'); -end + if length(varargin) == 0 + varargout{1} = geometry_wrapper(42, varargin{:}); + else + error('Arguments do not match any overload of function aGlobalFunction'); + end diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index e00004d57..b34112718 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -502,6 +502,19 @@ void aGlobalFunction_42(int nargout, mxArray *out[], int nargin, const mxArray * checkArguments("aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(aGlobalFunction()); } +void overloadedGlobalFunction_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,1); + int a = unwrap< int >(in[0]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a)); +} +void overloadedGlobalFunction_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,2); + int a = unwrap< int >(in[0]); + double b = unwrap< double >(in[1]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a,b)); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -643,6 +656,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 42: aGlobalFunction_42(nargout, out, nargin-1, in+1); break; + case 43: + overloadedGlobalFunction_43(nargout, out, nargin-1, in+1); + break; + case 44: + overloadedGlobalFunction_44(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/expected/overloadedGlobalFunction.m b/wrap/tests/expected/overloadedGlobalFunction.m new file mode 100644 index 000000000..5b086b15e --- /dev/null +++ b/wrap/tests/expected/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = geometry_wrapper(43, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') + varargout{1} = geometry_wrapper(44, varargin{:}); + else + error('Arguments do not match any overload of function overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index c7ab901ae..9c064734e 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -40,22 +40,12 @@ classdef ClassA < handle function varargout = memberFunction(this, varargin) % MEMBERFUNCTION usage: memberFunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % memberFunction() - if length(varargin) == 0 - varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.memberFunction'); - end + varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); end function varargout = nsArg(this, varargin) % NSARG usage: nsArg(ClassB arg) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsArg(ClassB arg) if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB') varargout{1} = testNamespaces_wrapper(10, this, varargin{:}); else @@ -66,14 +56,7 @@ classdef ClassA < handle function varargout = nsReturn(this, varargin) % NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsReturn(double q) - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.nsReturn'); - end + varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); end end diff --git a/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m new file mode 100644 index 000000000..84f3b8f47 --- /dev/null +++ b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'ns1.ClassA') + varargout{1} = testNamespaces_wrapper(24, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'ns1.ClassA') && isa(varargin{2},'double') + varargout{1} = testNamespaces_wrapper(25, varargin{:}); + else + error('Arguments do not match any overload of function ns2.overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 29739a2f6..6d22e1625 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -342,6 +342,21 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra checkArguments("ns2aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(ns2::aGlobalFunction()); } +void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false); +} +void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + double b = unwrap< double >(in[1]); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -426,6 +441,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 23: ns2aGlobalFunction_23(nargout, out, nargin-1, in+1); break; + case 24: + ns2overloadedGlobalFunction_24(nargout, out, nargin-1, in+1); + break; + case 25: + ns2overloadedGlobalFunction_25(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index bdced45ed..bc233763d 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -87,6 +87,10 @@ class Test { Vector aGlobalFunction(); +// An overloaded global function +Vector overloadedGlobalFunction(int a); +Vector overloadedGlobalFunction(int a, double b); + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index 8e6ef51d6..a9b23cad1 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -47,6 +47,10 @@ class ClassC { // separate namespace global function, same name Vector aGlobalFunction(); +// An overloaded global function +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a); +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a, double b); + } //\namespace ns2 class ClassD { diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 8bf2c1412..c6ce0903a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -15,16 +15,18 @@ * @author Frank Dellaert **/ +#include +#include + +#include + +#include +#include + #include #include #include #include -#include -#include -#include - -#include -#include using namespace std; using namespace boost::assign; @@ -305,8 +307,8 @@ TEST( wrap, parse_geometry ) { } // evaluate global functions -// Vector aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + // Vector aGlobalFunction(); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -380,7 +382,7 @@ TEST( wrap, parse_namespaces ) { // evaluate global functions // Vector ns1::aGlobalFunction(); // Vector ns2::aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -415,13 +417,17 @@ TEST( wrap, matlab_code_namespaces ) { module.matlab_code("actual_namespaces", headerPath); - EXPECT(files_equal(exp_path + "ClassD.m" , act_path + "ClassD.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassA.m" , act_path + "+ns1/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassB.m" , act_path + "+ns1/ClassB.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassA.m" , act_path + "+ns2/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassC.m" , act_path + "+ns2/ClassC.m" )); - EXPECT(files_equal(exp_path + "+ns2/+ns3/ClassB.m" , act_path + "+ns2/+ns3/ClassB.m" )); - EXPECT(files_equal(exp_path + "testNamespaces_wrapper.cpp" , act_path + "testNamespaces_wrapper.cpp" )); + EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassA.m", act_path + "+ns1/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassB.m", act_path + "+ns1/ClassB.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassA.m", act_path + "+ns2/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassC.m", act_path + "+ns2/ClassC.m" )); + EXPECT( + files_equal(exp_path + "+ns2/overloadedGlobalFunction.m", exp_path + "+ns2/overloadedGlobalFunction.m" )); + EXPECT( + files_equal(exp_path + "+ns2/+ns3/ClassB.m", act_path + "+ns2/+ns3/ClassB.m" )); + EXPECT( + files_equal(exp_path + "testNamespaces_wrapper.cpp", act_path + "testNamespaces_wrapper.cpp" )); } /* ************************************************************************* */ @@ -445,6 +451,7 @@ TEST( wrap, matlab_code_geometry ) { EXPECT(files_equal(epath + "Point3.m" , apath + "Point3.m" )); EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" )); EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.m" )); + EXPECT(files_equal(epath + "overloadedGlobalFunction.m" , apath + "overloadedGlobalFunction.m" )); } /* ************************************************************************* */ diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 1acc50db1..47f7d10a6 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -88,9 +88,11 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) string actual_contents = file_contents(actual, skipheader); bool equal = actual_contents == expected_contents; if (!equal) { + cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; - command << "diff " << actual << " " << expected << endl; + command << "diff --ignore-all-space " << expected << " " << actual << endl; system(command.str().c_str()); + cerr << ">>>\n"; } return equal; }