diff --git a/.cproject b/.cproject index 243637d65..cfdc18ef9 100644 --- a/.cproject +++ b/.cproject @@ -311,14 +311,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -345,6 +337,7 @@ make + tests/testBayesTree.run true false @@ -352,6 +345,7 @@ make + testBinaryBayesNet.run true false @@ -399,6 +393,7 @@ make + testSymbolicBayesNet.run true false @@ -406,6 +401,7 @@ make + tests/testSymbolicFactor.run true false @@ -413,6 +409,7 @@ make + testSymbolicFactorGraph.run true false @@ -428,11 +425,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -459,7 +465,6 @@ make - testGraph.run true false @@ -531,7 +536,6 @@ make - testInference.run true false @@ -539,7 +543,6 @@ make - testGaussianFactor.run true false @@ -547,7 +550,6 @@ make - testJunctionTree.run true false @@ -555,7 +557,6 @@ make - testSymbolicBayesNet.run true false @@ -563,7 +564,6 @@ make - testSymbolicFactorGraph.run true false @@ -633,22 +633,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -665,6 +649,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -689,18 +689,26 @@ true true - + make - -j2 VERBOSE=1 - check.nonlinear + -j2 + all true - false + true true - + make - -j5 - timing.nonlinear + -j2 + check + true + true + true + + + make + -j2 + clean true true true @@ -753,26 +761,34 @@ true true - + make - -j2 - all + -j2 VERBOSE=1 + check.nonlinear + true + false + true + + + make + -j5 + timing.nonlinear true true true - + make - -j2 - check + -j5 + nonlinear.testValues.run true true true - + make - -j2 - clean + -j5 + nonlinear.testOrdering.run true true true @@ -1123,6 +1139,7 @@ make + testErrors.run true false @@ -1602,7 +1619,6 @@ make - testSimulated2DOriented.run true false @@ -1642,7 +1658,6 @@ make - testSimulated2D.run true false @@ -1650,7 +1665,6 @@ make - testSimulated3D.run true false @@ -1744,10 +1758,10 @@ true true - + make - -j2 - tests/testNoiseModel.run + -j5 + linear.testNoiseModel.run true true true @@ -1824,6 +1838,14 @@ true true + + make + -j5 + linear.testSerializationLinear.run + true + true + true + make -j2 @@ -1914,7 +1936,6 @@ make - tests/testGaussianISAM2 true false @@ -1936,6 +1957,93 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + check + true + true + true + + + make + -j5 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j5 + gtsam-shared + true + true + true + + + make + -j5 + gtsam-static + true + true + true + + + make + -j5 + timing + true + true + true + + + make + -j5 + examples + true + true + true + + + make + -j5 + VERBOSE=1 all + true + true + true + + + make + -j5 + VERBOSE=1 check + true + true + true + make -j2 @@ -2032,23 +2140,7 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - + make -j2 check @@ -2056,7 +2148,23 @@ true true - + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + make -j2 all @@ -2064,46 +2172,13 @@ true true - + cmake .. - true false true - - make - -j5 - gtsam-shared - true - true - true - - - make - -j5 - gtsam-static - true - true - true - - - make - -j5 - timing - true - true - true - - - make - -j5 - examples - true - true - true - make -j5 @@ -2144,46 +2219,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - - true - false - true - diff --git a/CMakeLists.txt b/CMakeLists.txt index b8e560e7a..1067692dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,37 +2,58 @@ project(GTSAM CXX C) cmake_minimum_required(VERSION 2.6) # Set the version number for the library -set (GTSAM_VERSION_MAJOR 0) +set (GTSAM_VERSION_MAJOR 1) set (GTSAM_VERSION_MINOR 9) -set (GTSAM_VERSION_PATCH 3) +set (GTSAM_VERSION_PATCH 0) # Set the default install path to home set (CMAKE_INSTALL_PREFIX ${HOME} CACHE DOCSTRING "Install prefix for library") +# Use macros for creating tests/timing scripts +set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) +include(GtsamTesting) +include(GtsamPrinting) + # guard against in-source builds if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() -# guard against bad build-type strings -if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE "Debug") +# Default to Debug mode +if(NOT FIRST_PASS_DONE) + set(CMAKE_BUILD_TYPE "Debug" CACHE STRING + "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." + FORCE) endif() +# Check build types +if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS Debug Release Timing Profiling RelWithDebInfo MinSizeRel) +endif() string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower) if( NOT cmake_build_type_tolower STREQUAL "debug" AND NOT cmake_build_type_tolower STREQUAL "release" + AND NOT cmake_build_type_tolower STREQUAL "timing" + AND NOT cmake_build_type_tolower STREQUAL "profiling" AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo") - message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are Debug, Release, RelWithDebInfo (case-insensitive).") + message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).") endif() -# Add debugging flags -set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall") -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall") -set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -fno-inline -Wall -DNDEBUG -DEIGEN_NO_DEBUG") -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -fno-inline -Wall -DNDEBUG -DEIGEN_NO_DEBUG") -set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -DNDEBUG -Wall -DEIGEN_NO_DEBUG") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DNDEBUG -Wall -DEIGEN_NO_DEBUG") +# Add debugging flags but only on the first pass +if(NOT FIRST_PASS_DONE) + set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -fno-inline -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) + set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -fno-inline -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) + set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) + set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) + set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) + mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING) + set(CMAKE_C_FLAGS_PROFILING "-g -O2 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) + set(CMAKE_CXX_FLAGS_PROFILING "-g -O2 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) + mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING) +endif() # Configurable Options option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) @@ -41,6 +62,10 @@ option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF) option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" ON) +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON) +option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON) +option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON) +option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON) # Add the Quaternion Build Flag if requested if (GTSAM_USE_QUATERNIONS) @@ -59,10 +84,6 @@ if (GTSAM_BUILD_TESTS) include(CTest) endif() -# Use macros for creating tests/timing scripts -set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -include(GtsamTesting) - # Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND}) add_custom_target(timing) @@ -97,3 +118,48 @@ endif(GTSAM_BUILD_WRAP) if (GTSAM_BUILD_EXAMPLES) add_subdirectory(examples) endif(GTSAM_BUILD_EXAMPLES) + +# Mark that first pass is done +set(FIRST_PASS_DONE true CACHE BOOL "Internally used to mark whether cmake has been run multiple times") +mark_as_advanced(FIRST_PASS_DONE) + +# print configuration variables +message(STATUS "===============================================================") +message(STATUS "================ Configuration Options ======================") +message(STATUS "Build flags ") +print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") +print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ") +print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") +print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") +print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries") +string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper) +message(STATUS " Build type : ${CMAKE_BUILD_TYPE}") +message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") +message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") + +message(STATUS "GTSAM flags ") +print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3") + +message(STATUS "MATLAB toolbox flags ") +print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") +print_config_flag(${GTSAM_INSTALL_MATLAB_EXAMPLES} "Install matlab examples ") +print_config_flag(${GTSAM_INSTALL_MATLAB_TESTS} "Install matlab tests ") +print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ") +message(STATUS "===============================================================") + +# Set up CPack +set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") +set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") +set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README") +set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") +set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) +set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) +set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) +set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") +set(CPACK_INSTALLED_DIRECTORIES "doc" ".") # Include doc directory +set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makedoc.sh$") +set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") +#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs +set(CPACK_SOURCE_GENERATOR "TGZ") +set(CPACK_GENERATOR "TGZ") +include(CPack) diff --git a/cmake/FindCppUnitLite.cmake b/cmake/FindCppUnitLite.cmake index fba910218..57628b8fa 100644 --- a/cmake/FindCppUnitLite.cmake +++ b/cmake/FindCppUnitLite.cmake @@ -3,22 +3,19 @@ # The following variables will be defined: # # CppUnitLite_FOUND : TRUE if the package has been successfully found -# CppUnitLite_INCLUDE_DIRS : paths to CppUnitLite's INCLUDE directories +# CppUnitLite_INCLUDE_DIR : paths to CppUnitLite's INCLUDE directories # CppUnitLite_LIBS : paths to CppUnitLite's libraries - # Find include dirs find_path(_CppUnitLite_INCLUDE_DIR CppUnitLite/Test.h - PATHS ${GTSAM_ROOT} ${CMAKE_INSTALL_PREFIX}/include ${HOME}/include /usr/local/include /usr/include ) + PATHS ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" /usr/local/include /usr/include ) # Find libraries find_library(_CppUnitLite_LIB NAMES CppUnitLite - HINTS ${_CppUnitLite_INCLUDE_DIR}/build/CppUnitLite ${_CppUnitLite_INCLUDE_DIR}/CppUnitLite) - -set (CppUnitLite_INCLUDE_DIRS ${_CppUnitLite_INCLUDE_DIR}) -set (CppUnitLite_LIBS ${_CppUnitLite_LIB}) - + HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" /usr/local/lib /usr/lib) +set (CppUnitLite_INCLUDE_DIR ${_CppUnitLite_INCLUDE_DIR} CACHE STRING "CppUnitLite INCLUDE directories") +set (CppUnitLite_LIBS ${_CppUnitLite_LIB} CACHE STRING "CppUnitLite libraries") # handle the QUIETLY and REQUIRED arguments and set LIBXML2_FOUND to TRUE # if all listed variables are TRUE diff --git a/cmake/FindGTSAM.cmake b/cmake/FindGTSAM.cmake index 052798184..6748ae462 100644 --- a/cmake/FindGTSAM.cmake +++ b/cmake/FindGTSAM.cmake @@ -3,23 +3,19 @@ # The following variables will be defined: # # GTSAM_FOUND : TRUE if the package has been successfully found -# GTSAM_INCLUDE_DIRS : paths to GTSAM's INCLUDE directories +# GTSAM_INCLUDE_DIR : paths to GTSAM's INCLUDE directories # GTSAM_LIBS : paths to GTSAM's libraries - # Find include dirs find_path(_gtsam_INCLUDE_DIR gtsam/inference/FactorGraph.h - PATHS ${GTSAM_ROOT} ${CMAKE_INSTALL_PREFIX}/include ${HOME}/include /usr/local/include /usr/include ) + PATHS ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" /usr/local/include /usr/include ) # Find libraries find_library(_gtsam_LIB NAMES gtsam - HINTS ${_gtsam_INCLUDE_DIR}/build-debug/gtsam/.libs ${_gtsam_INCLUDE_DIR}/build/gtsam/.libs ${_gtsam_INCLUDE_DIR}/gtsam/.libs - NO_DEFAULT_PATH) - -set (GTSAM_INCLUDE_DIRS ${_gtsam_INCLUDE_DIR} CACHE STRING "GTSAM INCLUDE directories") -set (GTSAM_LIBS ${_gtsam_LIB} CACHE STRING "GTSAM libraries") - + HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" /usr/local/lib /usr/lib) +set (GTSAM_INCLUDE_DIR ${_gtsam_INCLUDE_DIR} CACHE STRING "GTSAM INCLUDE directories") +set (GTSAM_LIBS ${_gtsam_LIB} CACHE STRING "GTSAM libraries") # handle the QUIETLY and REQUIRED arguments and set LIBXML2_FOUND to TRUE # if all listed variables are TRUE diff --git a/cmake/FindWrap.cmake b/cmake/FindWrap.cmake new file mode 100644 index 000000000..733f6aa7d --- /dev/null +++ b/cmake/FindWrap.cmake @@ -0,0 +1,30 @@ +# This is FindWrap.cmake +# CMake module to locate the Wrap tool and header after installation package +# The following variables will be defined: +# +# Wrap_FOUND : TRUE if the package has been successfully found +# Wrap_CMD : command for executing wrap +# Wrap_INCLUDE_DIR : paths to Wrap's INCLUDE directories + +# Find include dir +find_path(_Wrap_INCLUDE_DIR wrap/matlab.h + PATHS ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" /usr/local/include /usr/include ) + +# Find the installed executable +find_program(_Wrap_CMD NAMES wrap + PATHS ${CMAKE_INSTALL_PREFIX}/bin "$ENV{HOME}/bin" /usr/local/bin /usr/bin ) + +set (Wrap_INCLUDE_DIR ${_Wrap_INCLUDE_DIR} CACHE STRING "Wrap INCLUDE directories") +set (Wrap_CMD ${_Wrap_CMD} CACHE STRING "Wrap executable location") + +# handle the QUIETLY and REQUIRED arguments and set LIBXML2_FOUND to TRUE +# if all listed variables are TRUE +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Wrap DEFAULT_MSG + _Wrap_INCLUDE_DIR _Wrap_CMD) + +mark_as_advanced(_Wrap_INCLUDE_DIR _Wrap_CMD ) + + + + diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake new file mode 100644 index 000000000..8c2098b1d --- /dev/null +++ b/cmake/GtsamMatlabWrap.cmake @@ -0,0 +1,48 @@ +# Macros for using wrap functionality +macro(find_mexextension) + ## Determine the mex extension + # Apple Macintosh (64-bit) mexmaci64 + # Linux (32-bit) mexglx + # Linux (64-bit) mexa64 + # Microsoft Windows (32-bit) mexw32 + # Windows (64-bit) mexw64 + + # only support 64-bit apple + if(CMAKE_HOST_APPLE) + set(GTSAM_MEX_BIN_EXTENSION_default mexmaci64) + endif(CMAKE_HOST_APPLE) + + if(NOT CMAKE_HOST_APPLE) + # check 64 bit + if( ${CMAKE_SIZEOF_VOID_P} EQUAL 4 ) + set( HAVE_64_BIT 0 ) + endif( ${CMAKE_SIZEOF_VOID_P} EQUAL 4 ) + + if( ${CMAKE_SIZEOF_VOID_P} EQUAL 8 ) + set( HAVE_64_BIT 1 ) + endif( ${CMAKE_SIZEOF_VOID_P} EQUAL 8 ) + + # Check for linux machines + if (CMAKE_HOST_UNIX) + if (HAVE_64_BIT) + set(GTSAM_MEX_BIN_EXTENSION_default mexa64) + else (HAVE_64_BIT) + set(GTSAM_MEX_BIN_EXTENSION_default mexglx) + endif (HAVE_64_BIT) + endif(CMAKE_HOST_UNIX) + + # Check for windows machines + if (CMAKE_HOST_WIN32) + if (HAVE_64_BIT) + set(GTSAM_MEX_BIN_EXTENSION_default mexw64) + else (HAVE_64_BIT) + set(GTSAM_MEX_BIN_EXTENSION_default mexw32) + endif (HAVE_64_BIT) + endif(CMAKE_HOST_WIN32) + endif(NOT CMAKE_HOST_APPLE) + + # Allow for setting mex extension manually + set(GTSAM_MEX_BIN_EXTENSION ${GTSAM_MEX_BIN_EXTENSION_default} CACHE DOCSTRING "Extension for matlab mex files") + message(STATUS "Detected Matlab mex extension: ${GTSAM_MEX_BIN_EXTENSION_default}") + message(STATUS "Current Matlab mex extension: ${GTSAM_MEX_BIN_EXTENSION}") +endmacro(find_mexextension) \ No newline at end of file diff --git a/cmake/GtsamPrinting.cmake b/cmake/GtsamPrinting.cmake new file mode 100644 index 000000000..b6f3ca4ef --- /dev/null +++ b/cmake/GtsamPrinting.cmake @@ -0,0 +1,10 @@ +# print configuration variables +# Usage: +#print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") +macro(print_config_flag flag msg) + if ("${flag}" STREQUAL "ON") + message(STATUS " ${msg}: Enabled") + else ("${flag}" STREQUAL "ON") + message(STATUS " ${msg}: Disabled") + endif ("${flag}" STREQUAL "ON") +endmacro(print_config_flag) \ No newline at end of file diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index f2f345f11..28683c98b 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -6,7 +6,7 @@ macro(gtsam_add_tests subdir libs) file(GLOB tests_srcs "tests/test*.cpp") foreach(test_src ${tests_srcs}) get_filename_component(test_base ${test_src} NAME_WE) - set( test_bin ${subdir}.${test_base} ) + set( test_bin ${test_base} ) message(STATUS "Adding Test ${test_bin}") add_executable(${test_bin} ${test_src}) add_dependencies(check.${subdir} ${test_bin}) @@ -30,7 +30,7 @@ macro(gtsam_add_external_tests subdir libs) file(GLOB tests_srcs "tests/test*.cpp") foreach(test_src ${tests_srcs}) get_filename_component(test_base ${test_src} NAME_WE) - set( test_bin ${subdir}.${test_base} ) + set( test_bin ${test_base} ) message(STATUS "Adding Test ${test_bin}") add_executable(${test_bin} ${test_src}) add_dependencies(check.${subdir} ${test_bin}) @@ -48,7 +48,7 @@ macro(gtsam_add_timing subdir libs) file(GLOB base_timing_srcs "tests/time*.cpp") foreach(time_src ${base_timing_srcs}) get_filename_component(time_base ${time_src} NAME_WE) - set( time_bin ${subdir}.${time_base} ) + set( time_bin ${time_base} ) message(STATUS "Adding Timing Benchmark ${time_bin}") add_executable(${time_bin} ${time_src}) add_dependencies(timing.${subdir} ${time_bin}) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 27f85b2f7..55f2db60b 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -9,6 +9,7 @@ foreach(example_src ${example_srcs} ) add_dependencies(examples ${example_bin}) add_executable(${example_bin} ${example_src}) target_link_libraries(${example_bin} gtsam-static) + add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) endforeach(example_src) add_subdirectory(vSLAMexample) diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 65b201ab7..10802ee87 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -39,13 +39,13 @@ #include #include -// Main typedefs -typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain -typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain - using namespace std; using namespace gtsam; +// Main typedefs +typedef NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain +typedef NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain + /** * In this version of the system we make the following assumptions: * - All values are axis aligned diff --git a/examples/matlab/Pose2SLAMExample_easy.m b/examples/matlab/Pose2SLAMExample_easy.m index bbafba99d..6c359f314 100644 --- a/examples/matlab/Pose2SLAMExample_easy.m +++ b/examples/matlab/Pose2SLAMExample_easy.m @@ -39,7 +39,7 @@ graph.addOdometry(x2, x3, odom_measurement, odom_model); %% Add measurements % general noisemodel for measurements -meas_model = SharedNoiseModel_sharedSigmas([0.1; 0.2]); +meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]); % print graph.print('full graph'); diff --git a/gtsam.h b/gtsam.h index 688fc4fa7..26f3457c7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -214,7 +214,7 @@ class CalibratedCamera { CalibratedCamera(const Vector& v); void print(string s) const; - bool equals(const gtsam::Pose3& pose, double tol) const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; gtsam::Pose3 pose() const; @@ -404,7 +404,7 @@ class Ordering { Ordering(); void print(string s) const; bool equals(const gtsam::Ordering& ord, double tol) const; - void push_back(string key); + void push_back(size_t key); }; class NonlinearOptimizationParameters { diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 51439c6f1..505f06921 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -17,6 +17,7 @@ #pragma once +#include #include namespace gtsam { @@ -24,7 +25,7 @@ namespace gtsam { /** * LieScalar is a wrapper around double to allow it to be a Lie type */ - struct LieScalar { + struct LieScalar : public DerivedValue { /** default constructor */ LieScalar() : d_(0.0) {} diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 46f738f7d..dad1c6d20 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -70,6 +70,14 @@ bool assert_equal(const V& expected, const boost::optional& actual, double to return assert_equal(expected, *actual, tol); } +template +bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { + if (!actual) { + std::cout << "actual is boost::none" << std::endl; + return false; + } + return assert_equal(expected, *actual, tol); +} /** * Version of assert_equals to work with vectors @@ -247,6 +255,45 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e- return true; } +/** + * Function for comparing maps of size_t->testable + * Types are assumed to have operator == + */ +template +bool assert_container_equality(const std::map& expected, const std::map& actual) { + typedef typename std::map Map; + bool match = true; + if (expected.size() != actual.size()) + match = false; + typename Map::const_iterator + itExp = expected.begin(), + itAct = actual.begin(); + if(match) { + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (itExp->first != itAct->first || itExp->second != itAct->second) { + match = false; + break; + } + } + } + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, expected) { + std::cout << "Key: " << a.first << std::endl; + std::cout << "Value: " << a.second << std::endl; + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, actual) { + std::cout << "Key: " << a.first << std::endl; + std::cout << "Value: " << a.second << std::endl; + } + std::cout << std::endl; + return false; + } + return true; +} + + /** * General function for comparing containers of objects with operator== */ diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 684e28ae0..a05440d69 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -74,6 +74,12 @@ public: /** Default constructor as an empty BayesNet */ BayesNet() {}; + /** convert from a derived type */ + template + BayesNet(const BayesNet& bn) { + conditionals_.assign(bn.begin(), bn.end()); + } + /** BayesNet with 1 conditional */ BayesNet(const sharedConditional& conditional) { push_back(conditional); } diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 23148cea8..95995a4e6 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -234,15 +234,56 @@ namespace gtsam { } } - /* ************************************************************************* */ + /* ************************************************************************* */ template - BayesTree::BayesTree() { + void BayesTree::recursiveTreeBuild(const boost::shared_ptr >& symbolic, + const std::vector >& conditionals, + const typename BayesTree::sharedClique& parent) { + + // Helper function to build a non-symbolic tree (e.g. Gaussian) using a + // symbolic tree, used in the BT(BN) constructor. + + // Build the current clique + FastList cliqueConditionals; + BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) { + cliqueConditionals.push_back(conditionals[j]); } + typename BayesTree::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end()))); + + // Add the new clique with the current parent + this->addClique(thisClique, parent); + + // Build the children, whose parent is the new clique + BOOST_FOREACH(const BayesTree::sharedClique& child, symbolic->children()) { + this->recursiveTreeBuild(child, conditionals, thisClique); } + } + + /* ************************************************************************* */ + template + BayesTree::BayesTree(const BayesNet& bayesNet) { + // First generate symbolic BT to determine clique structure + BayesTree sbt(bayesNet); + + // Build index of variables to conditionals + std::vector > conditionals(sbt.root()->conditional()->frontals().back() + 1); + BOOST_FOREACH(const boost::shared_ptr& c, bayesNet) { + if(c->nrFrontals() != 1) + throw std::invalid_argument("BayesTree constructor from BayesNet only supports single frontal variable conditionals"); + if(c->firstFrontalKey() >= conditionals.size()) + throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTree constructor!"); + if(conditionals[c->firstFrontalKey()]) + throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTree constructor!"); + + conditionals[c->firstFrontalKey()] = c; + } + + // Build the new tree + this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique()); } /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet) { - typename BayesNet::const_reverse_iterator rit; + template<> + inline BayesTree::BayesTree(const BayesNet& bayesNet) { + BayesNet::const_reverse_iterator rit; for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) insert(*this, *rit); } @@ -311,19 +352,6 @@ namespace gtsam { typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); assert(lowestOrderedParent != parents.end()); return *lowestOrderedParent; - -// boost::optional parentCliqueRepresentative; -// boost::optional lowest; -// BOOST_FOREACH(Index p, parents) { -// size_t i = index(p); -// if (!lowest || i<*lowest) { -// lowest.reset(i); -// parentCliqueRepresentative.reset(p); -// } -// } -// if (!lowest) throw -// invalid_argument("BayesTree::findParentClique: no parents given or key not present in index"); -// return *parentCliqueRepresentative; } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 87788e3d8..06a47ba8e 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -31,6 +31,7 @@ #include #include #include +#include #include namespace gtsam { @@ -127,15 +128,22 @@ namespace gtsam { /** Fill the nodes index for a subtree */ void fillNodesIndex(const sharedClique& subtree); + /** Helper function to build a non-symbolic tree (e.g. Gaussian) using a + * symbolic tree, used in the BT(BN) constructor. + */ + void recursiveTreeBuild(const boost::shared_ptr >& symbolic, + const std::vector >& conditionals, + const typename BayesTree::sharedClique& parent); + public: /// @name Standard Constructors /// @{ /** Create an empty Bayes Tree */ - BayesTree(); + BayesTree() {} - /** Create a Bayes Tree from a Bayes Net */ + /** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */ BayesTree(const BayesNet& bayesNet); /// @} diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 08427bae4..82cec6f76 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index d05cc1de3..d9fb1e0de 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -45,12 +45,12 @@ namespace gtsam { void Factor::assertInvariants() const { #ifndef NDEBUG // Check that keys are all unique - std::multiset < KeyType > nonunique(keys_.begin(), keys_.end()); - std::set < KeyType > unique(keys_.begin(), keys_.end()); + std::multiset nonunique(keys_.begin(), keys_.end()); + std::set unique(keys_.begin(), keys_.end()); bool correct = (nonunique.size() == unique.size()) && std::equal(nonunique.begin(), nonunique.end(), unique.begin()); if (!correct) throw std::logic_error( - "Factor::assertInvariants: detected inconsistency"); + "Factor::assertInvariants: Factor contains duplicate keys"); #endif } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 962bf44d1..87c1f3860 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -117,6 +117,14 @@ public: Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } + /** Construct 5-way factor */ + Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5) : keys_(5) { + keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; assertInvariants(); } + + /** Construct 6-way factor */ + Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5, KeyType key6) : keys_(6) { + keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; assertInvariants(); } + /// @} /// @name Advanced Constructors /// @{ diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 12855e0c0..33cdd02a1 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -43,6 +43,7 @@ template class BayesTree; class FactorGraph { public: typedef FACTOR FactorType; + typedef typename FACTOR::KeyType KeyType; typedef boost::shared_ptr > shared_ptr; typedef typename boost::shared_ptr sharedFactor; typedef typename std::vector::iterator iterator; @@ -56,6 +57,11 @@ template class BayesTree; /** typedef for an eliminate subroutine */ typedef boost::function&, size_t)> Eliminate; + /** Typedef for the result of factorization */ + typedef std::pair< + boost::shared_ptr, + FactorGraph > FactorizationResult; + protected: /** concept check */ @@ -87,7 +93,7 @@ template class BayesTree; /** convert from a derived type */ template FactorGraph(const FactorGraph& factors) { - factors_.insert(end(), factors.begin(), factors.end()); + factors_.assign(factors.begin(), factors.end()); } /// @} diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 04e8cfbeb..b2629566b 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -89,7 +89,7 @@ namespace gtsam { const std::vector& js, Eliminate function) const { // Compute a COLAMD permutation with the marginal variable constrained to the end. - Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js)); + Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js)); Permutation::shared_ptr permutationInverse(permutation->inverse()); // Permute the factors - NOTE that this permutes the original factors, not diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index 58d5f93d1..9830782f3 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -31,12 +31,12 @@ namespace gtsam { // Checks for uniqueness of keys Base::assertInvariants(); #ifndef NDEBUG - // Check that separator keys are sorted - FastSet uniquesorted(beginFrontals(), endFrontals()); - assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals())); - // Check that separator keys are less than parent keys - //BOOST_FOREACH(Index j, frontals()) { - // assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); } + // Check that frontal keys are sorted + //FastSet uniquesorted(beginFrontals(), endFrontals()); + //assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals())); + //// Check that separator keys are less than parent keys + ////BOOST_FOREACH(Index j, frontals()) { + //// assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); } #endif } @@ -60,13 +60,13 @@ namespace gtsam { /* ************************************************************************* */ void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) { // The permutation may not move the separators into the frontals - #ifndef NDEBUG - BOOST_FOREACH(const KeyType frontal, this->frontals()) { - BOOST_FOREACH(const KeyType separator, this->parents()) { - assert(inversePermutation[frontal] < inversePermutation[separator]); - } - } - #endif +// #ifndef NDEBUG +// BOOST_FOREACH(const KeyType frontal, this->frontals()) { +// BOOST_FOREACH(const KeyType separator, this->parents()) { +// assert(inversePermutation[frontal] < inversePermutation[separator]); +// } +// } +// #endif BOOST_FOREACH(Index& key, keys()) key = inversePermutation[key]; assertInvariants(); diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 5cbd8e187..dc3aa669a 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -26,8 +26,6 @@ namespace gtsam { -class Inference; - /** * A permutation reorders variables, for example to reduce fill-in during * elimination. To save computation, the permutation can be applied to @@ -162,8 +160,6 @@ protected: void check(Index variable) const { assert(variable < rangeIndices_.size()); } /// @} - - friend class Inference; }; diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index fc4fde620..878f6a85a 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -26,8 +26,6 @@ namespace gtsam { -class Inference; - /** * The VariableIndex class computes and stores the block column structure of a * factor graph. The factor graph stores a collection of factors, each of diff --git a/gtsam/inference/inference-inl.h b/gtsam/inference/inference-inl.h new file mode 100644 index 000000000..02e78e8e9 --- /dev/null +++ b/gtsam/inference/inference-inl.h @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * 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 inference-inl.h + * @brief + * @author Richard Roberts + * @date Mar 3, 2012 + */ + +#pragma once + +#include + +// Only for Eclipse parser, inference-inl.h (this file) is included at the bottom of inference.h +#include + +#include + +namespace gtsam { + +namespace inference { + +/* ************************************************************************* */ +template +Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) { + + std::vector cmember(variableIndex.size(), 0); + + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + if(constrainLast.size() < variableIndex.size()) { + BOOST_FOREACH(Index var, constrainLast) { + assert(var < variableIndex.size()); + cmember[var] = 1; + } + } + + return PermutationCOLAMD_(variableIndex, cmember); +} + +/* ************************************************************************* */ +inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) { + std::vector cmember(variableIndex.size(), 0); + return PermutationCOLAMD_(variableIndex, cmember); +} + +/* ************************************************************************* */ +template +typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector& variables, + const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex_) { + + const VariableIndex& variableIndex = variableIndex_ ? *variableIndex_ : VariableIndex(factorGraph); + + // First find the involved factors + Graph involvedFactors; + Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors + + // First get the indices of the involved factors, but uniquely in a set + FastSet involvedFactorIndices; + BOOST_FOREACH(Index variable, variables) { + involvedFactorIndices.insert(variableIndex[variable].begin(), variableIndex[variable].end()); } + + // Add the factors themselves to involvedFactors and update largest index + involvedFactors.reserve(involvedFactorIndices.size()); + BOOST_FOREACH(size_t factorIndex, involvedFactorIndices) { + const typename Graph::sharedFactor factor = factorGraph[factorIndex]; + involvedFactors.push_back(factor); // Add involved factor + highestInvolvedVariable = std::max( // Updated largest index + highestInvolvedVariable, + *std::max_element(factor->begin(), factor->end())); + } + + // Now permute the variables to be eliminated to the front of the ordering + Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1); + Permutation toFrontInverse = *toFront.inverse(); + involvedFactors.permuteWithInverse(toFrontInverse); + + // Eliminate into conditional and remaining factor + typename Graph::EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size()); + boost::shared_ptr conditional = eliminated.first; + typename Graph::sharedFactor remainingFactor = eliminated.second; + + // Undo the permutation + conditional->permuteWithInverse(toFront); + remainingFactor->permuteWithInverse(toFront); + + // Build the remaining graph, without the removed factors + Graph remainingGraph; + remainingGraph.reserve(factorGraph.size() - involvedFactors.size() + 1); + FastSet::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin(); + for(size_t i = 0; i < factorGraph.size(); ++i) { + if(involvedFactorIndexIt != involvedFactorIndices.end() && *involvedFactorIndexIt == i) + ++ involvedFactorIndexIt; + else + remainingGraph.push_back(factorGraph[i]); + } + + // Add the remaining factor if it is not empty. + if(remainingFactor->size() != 0) + remainingGraph.push_back(remainingFactor); + + return typename Graph::FactorizationResult(conditional, remainingGraph); + +} + + +} + +} + + diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp index d30ea79c4..e1c250fcf 100644 --- a/gtsam/inference/inference.cpp +++ b/gtsam/inference/inference.cpp @@ -29,7 +29,9 @@ using namespace std; namespace gtsam { -Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember) { +namespace inference { + +Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember) { size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); // Convert to compressed column major format colamd wants it in (== MATLAB format!) int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */ @@ -79,10 +81,10 @@ Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& varia // Convert elimination ordering in p to an ordering Permutation::shared_ptr permutation(new Permutation(nVars)); for (Index j = 0; j < nVars; j++) { -// if(p[j] == -1) -// permutation->operator[](j) = j; -// else - permutation->operator[](j) = p[j]; + // if(p[j] == -1) + // permutation->operator[](j) = j; + // else + permutation->operator[](j) = p[j]; if(debug) cout << "COLAMD: " << j << "->" << p[j] << endl; } if(debug) cout << "COLAMD: p[" << nVars << "] = " << p[nVars] << endl; @@ -91,3 +93,5 @@ Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& varia } } + +} diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h index d853ff91b..d87720b5a 100644 --- a/gtsam/inference/inference.h +++ b/gtsam/inference/inference.h @@ -22,58 +22,59 @@ #include #include +#include #include namespace gtsam { - class Inference { - private: - /* Static members only, private constructor */ - Inference() {} +namespace inference { - public: +/** + * Compute a permutation (variable ordering) using colamd + */ +Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex); - /** - * Compute a permutation (variable ordering) using colamd - */ - static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex); +/** + * Compute a permutation (variable ordering) using constrained colamd + */ +template +Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast); - /** - * Compute a permutation (variable ordering) using constrained colamd - */ - template - static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast); +/** + * Compute a CCOLAMD permutation using the constraint groups in cmember. + */ +Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember); - /** - * Compute a CCOLAMD permutation using the constraint groups in cmember. - */ - static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember); +/** Factor the factor graph into a conditional and a remaining factor graph. + * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out + * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where + * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is + * a probability density or likelihood, the factorization produces a + * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. + * + * For efficiency, this function treats the variables to eliminate + * \c variables as fully-connected, so produces a dense (fully-connected) + * conditional on all of the variables in \c variables, instead of a sparse + * BayesNet. If the variables are not fully-connected, it is more efficient + * to sequentially factorize multiple times. + */ +template +typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector& variables, + const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex = boost::none); - }; +/** Eliminate a single variable, by calling + * eliminate(const Graph&, const std::vector&, const typename Graph::Eliminate&, boost::optional) + */ +template +typename Graph::FactorizationResult eliminateOne(const Graph& factorGraph, typename Graph::KeyType variable, + const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex = boost::none) { + std::vector variables(1, variable); + return eliminate(factorGraph, variables, eliminateFcn, variableIndex); +} - /* ************************************************************************* */ - template - Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) { - - std::vector cmember(variableIndex.size(), 0); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - if(constrainLast.size() < variableIndex.size()) { - BOOST_FOREACH(Index var, constrainLast) { - assert(var < variableIndex.size()); - cmember[var] = 1; - } - } - - return PermutationCOLAMD_(variableIndex, cmember); - } - - /* ************************************************************************* */ - inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) { - std::vector cmember(variableIndex.size(), 0); - return PermutationCOLAMD_(variableIndex, cmember); - } +} } // namespace gtsam + +#include diff --git a/gtsam/inference/tests/testInference.cpp b/gtsam/inference/tests/testInference.cpp index 697ee9ec2..41c33a456 100644 --- a/gtsam/inference/tests/testInference.cpp +++ b/gtsam/inference/tests/testInference.cpp @@ -25,7 +25,7 @@ using namespace gtsam; /* ************************************************************************* */ -TEST(Inference, UnobservedVariables) { +TEST(inference, UnobservedVariables) { SymbolicFactorGraph sfg; // Create a factor graph that skips some variables @@ -35,7 +35,7 @@ TEST(Inference, UnobservedVariables) { VariableIndex variableIndex(sfg); - Permutation::shared_ptr colamd(Inference::PermutationCOLAMD(variableIndex)); + Permutation::shared_ptr colamd(inference::PermutationCOLAMD(variableIndex)); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index e05fbe45c..5721a4760 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -83,33 +83,14 @@ boost::shared_ptr allocateVectorValues(const GaussianBayesNet& bn) /* ************************************************************************* */ VectorValues optimize(const GaussianBayesNet& bn) { - return *optimize_(bn); -} - -/* ************************************************************************* */ -boost::shared_ptr optimize_(const GaussianBayesNet& bn) -{ - // get the RHS as a VectorValues to initialize system - boost::shared_ptr result(new VectorValues(rhs(bn))); - - /** solve each node in turn in topological sort order (parents first)*/ - BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) - cg->solveInPlace(*result); // solve and store solution in same step - - return result; -} - -/* ************************************************************************* */ -VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y) { - VectorValues x(y); - backSubstituteInPlace(bn,x); + VectorValues x = *allocateVectorValues(bn); + optimizeInPlace(bn, x); return x; } /* ************************************************************************* */ // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) -void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y) { - VectorValues& x = y; +void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) { /** solve each node in turn in topological sort order (parents first)*/ BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, bn) { // i^th part of R*x=y, x=inv(R)*y @@ -141,6 +122,42 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn, return gy; } +/* ************************************************************************* */ +VectorValues optimizeGradientSearch(const GaussianBayesNet& Rd) { + tic(0, "Allocate VectorValues"); + VectorValues grad = *allocateVectorValues(Rd); + toc(0, "Allocate VectorValues"); + + optimizeGradientSearchInPlace(Rd, grad); + + return grad; +} + +/* ************************************************************************* */ +void optimizeGradientSearchInPlace(const GaussianBayesNet& Rd, VectorValues& grad) { + tic(1, "Compute Gradient"); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(Rd, grad); + double gradientSqNorm = grad.dot(grad); + toc(1, "Compute Gradient"); + + tic(2, "Compute R*g"); + // Compute R * g + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; + toc(2, "Compute R*g"); + + tic(3, "Compute minimizing step size"); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + toc(3, "Compute minimizing step size"); + + tic(4, "Compute point"); + // Compute steepest descent point + scal(step, grad); + toc(4, "Compute point"); +} + /* ************************************************************************* */ pair matrix(const GaussianBayesNet& bn) { @@ -194,15 +211,6 @@ pair matrix(const GaussianBayesNet& bn) { return make_pair(R,d); } -/* ************************************************************************* */ -VectorValues rhs(const GaussianBayesNet& bn) { - boost::shared_ptr result(allocateVectorValues(bn)); - BOOST_FOREACH(boost::shared_ptr cg,bn) - cg->rhs(*result); - - return *result; -} - /* ************************************************************************* */ double determinant(const GaussianBayesNet& bayesNet) { double logDet = 0.0; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 455536a4f..1ef9081d4 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -55,26 +55,56 @@ namespace gtsam { boost::shared_ptr allocateVectorValues(const GaussianBayesNet& bn); /** - * optimize, i.e. return x = inv(R)*d + * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by + * back-substitution. */ - VectorValues optimize(const GaussianBayesNet&); + VectorValues optimize(const GaussianBayesNet& bn); - /** - * shared pointer version - */ - boost::shared_ptr optimize_(const GaussianBayesNet& bn); + /** + * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by + * back-substitution, writes the solution \f$ x \f$ into a pre-allocated + * VectorValues. You can use allocateVectorValues(const GaussianBayesNet&) + * allocate it. See also optimize(const GaussianBayesNet&), which does not + * require pre-allocation. + */ + void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x); - /** - * Backsubstitute - * (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) - * @param y is the RHS of the system - */ - VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y); + /** + * Optimize along the gradient direction, with a closed-form computation to + * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized + * problem. The error function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the + * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size + * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields + * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function + * evaluates the denominator without computing the Hessian \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] + * + * @param bn The GaussianBayesNet on which to perform this computation + * @return The resulting \f$ \delta x \f$ as described above + */ + VectorValues optimizeGradientSearch(const GaussianBayesNet& bn); - /** - * Backsubstitute in place, y starts as RHS and is replaced with solution - */ - void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y); + /** In-place version of optimizeGradientSearch(const GaussianBayesNet&) requiring pre-allocated VectorValues \c grad + * + * @param bn The GaussianBayesNet on which to perform this computation + * @param [out] grad The resulting \f$ \delta x \f$ as described in optimizeGradientSearch(const GaussianBayesNet&) + * */ + void optimizeGradientSearchInPlace(const GaussianBayesNet& bn, VectorValues& grad); /** * Transpose Backsubstitute @@ -91,12 +121,6 @@ namespace gtsam { */ std::pair matrix(const GaussianBayesNet&); - /** - * Return RHS d as a VectorValues - * Such that backSubstitute(bn,d) = optimize(bn) - */ - VectorValues rhs(const GaussianBayesNet&); - /** * Computes the determinant of a GassianBayesNet * A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h new file mode 100644 index 000000000..9cb327bec --- /dev/null +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -0,0 +1,28 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianBayesTree.cpp + * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree + * @brief GaussianBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include + +#include // Only to help Eclipse + +namespace gtsam { + +} diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp new file mode 100644 index 000000000..4243657e5 --- /dev/null +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianBayesTree.cpp + * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree + * @brief GaussianBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +namespace internal { +void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result) { + // parents are assumed to already be solved and available in result + clique->conditional()->solveInPlace(result); + + // starting from the root, call optimize on each conditional + BOOST_FOREACH(const boost::shared_ptr >& child, clique->children_) + optimizeInPlace(child, result); +} +} + +/* ************************************************************************* */ +VectorValues optimize(const GaussianBayesTree& bayesTree) { + VectorValues result = *allocateVectorValues(bayesTree); + internal::optimizeInPlace(bayesTree.root(), result); + return result; +} + +/* ************************************************************************* */ +VectorValues optimizeGradientSearch(const GaussianBayesTree& Rd) { + tic(0, "Allocate VectorValues"); + VectorValues grad = *allocateVectorValues(Rd); + toc(0, "Allocate VectorValues"); + + optimizeGradientSearchInPlace(Rd, grad); + + return grad; +} + +/* ************************************************************************* */ +void optimizeGradientSearchInPlace(const GaussianBayesTree& Rd, VectorValues& grad) { + tic(1, "Compute Gradient"); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(Rd, grad); + double gradientSqNorm = grad.dot(grad); + toc(1, "Compute Gradient"); + + tic(2, "Compute R*g"); + // Compute R * g + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; + toc(2, "Compute R*g"); + + tic(3, "Compute minimizing step size"); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + toc(3, "Compute minimizing step size"); + + tic(4, "Compute point"); + // Compute steepest descent point + scal(step, grad); + toc(4, "Compute point"); +} + +/* ************************************************************************* */ +void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { + internal::optimizeInPlace(bayesTree.root(), result); +} + +/* ************************************************************************* */ +VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree), x0); +} + +/* ************************************************************************* */ +void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) { + gradientAtZero(FactorGraph(bayesTree), g); +} + +} + + + + diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h new file mode 100644 index 000000000..bb15eb063 --- /dev/null +++ b/gtsam/linear/GaussianBayesTree.h @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianBayesTree.h + * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree + * @brief GaussianBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +typedef BayesTree GaussianBayesTree; + +/// optimize the BayesTree, starting from the root +VectorValues optimize(const GaussianBayesTree& bayesTree); + +/// recursively optimize this conditional and all subtrees +void optimizeInPlace(const GaussianBayesTree& clique, VectorValues& result); + +namespace internal { +void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result); +} + +/** + * Optimize along the gradient direction, with a closed-form computation to + * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized + * problem. The error function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the + * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size + * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields + * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function + * evaluates the denominator without computing the Hessian \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] + */ +VectorValues optimizeGradientSearch(const GaussianBayesTree& bn); + +/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ +void optimizeGradientSearchInPlace(const GaussianBayesTree& bn, VectorValues& grad); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ R^T(Rx-d) \f$. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ +VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around zero. + * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ +void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g); + +} + +#include + diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 7bff355d7..233dc4b34 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -31,19 +31,19 @@ namespace gtsam { // Helper function used only in this file - extracts vectors with variable indices // in the first and last iterators, and concatenates them in that order into the // output. -template -static Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last) { +template +static Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) { // Find total dimensionality int dim = 0; for(ITERATOR j = first; j != last; ++j) - dim += values.dim(*j); + dim += values[*j].rows(); // Copy vectors Vector ret(dim); int varStart = 0; for(ITERATOR j = first; j != last; ++j) { - ret.segment(varStart, values.dim(*j)) = values[*j]; - varStart += values.dim(*j); + ret.segment(varStart, values[*j].rows()) = values[*j]; + varStart += values[*j].rows(); } return ret; } @@ -52,15 +52,15 @@ static Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR fir // Helper function used only in this file - writes to the variables in values // with indices iterated over by first and last, interpreting vector as the // concatenated vectors to write. -template -static void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) { +template +static void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) { // Copy vectors int varStart = 0; for(ITERATOR j = first; j != last; ++j) { - values[*j] = vector.segment(varStart, values.dim(*j)); - varStart += values.dim(*j); + values[*j] = vector.segment(varStart, values[*j].rows()); + varStart += values[*j].rows(); } - assert(varStart = vector.rows()); + assert(varStart == vector.rows()); } /* ************************************************************************* */ @@ -221,78 +221,34 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const { } /* ************************************************************************* */ -void GaussianConditional::rhs(VectorValues& x) const { - writeVectorValuesSlices(get_d(), x, beginFrontals(), endFrontals()); -} +template +inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES& x) { -/* ************************************************************************* */ -void GaussianConditional::rhs(Permuted& x) const { - // Copy the rhs into x, accounting for the permutation - Vector d = get_d(); - size_t rhsPosition = 0; // We walk through the rhs by variable - for(const_iterator j = beginFrontals(); j != endFrontals(); ++j) { - // Get the segment of the rhs for this variable - x[*j] = d.segment(rhsPosition, this->dim(j)); - // Increment the position - rhsPosition += this->dim(j); + // Helper function to solve-in-place on a VectorValues or Permuted, + // called by GaussianConditional::solveInPlace(VectorValues&) and by + // GaussianConditional::solveInPlace(Permuted&). + + static const bool debug = false; + if(debug) conditional.print("Solving conditional in place"); + Vector xS = extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents()); + xS = conditional.get_d() - conditional.get_S() * xS; + Vector soln = conditional.permutation().transpose() * + conditional.get_R().triangularView().solve(xS); + if(debug) { + gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on "); + gtsam::print(soln, "full back-substitution solution: "); } + writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals()); } /* ************************************************************************* */ void GaussianConditional::solveInPlace(VectorValues& x) const { - static const bool debug = false; - if(debug) print("Solving conditional in place"); - Vector rhs = extractVectorValuesSlices(x, beginFrontals(), endFrontals()); - for (const_iterator parent = beginParents(); parent != endParents(); ++parent) { - rhs += -get_S(parent) * x[*parent]; - } - Vector soln = permutation_.transpose() * get_R().triangularView().solve(rhs); - if(debug) { - gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on "); - gtsam::print(rhs, "rhs: "); - gtsam::print(soln, "full back-substitution solution: "); - } - writeVectorValuesSlices(soln, x, beginFrontals(), endFrontals()); + doSolveInPlace(*this, x); // Call helper version above } /* ************************************************************************* */ void GaussianConditional::solveInPlace(Permuted& x) const { - static const bool debug = false; - if(debug) print("Solving conditional in place (permuted)"); - // Extract RHS from values - inlined from VectorValues - size_t s = 0; - for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it) - s += x[*it].size(); - Vector rhs(s); size_t start = 0; - for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it) { - SubVector v = x[*it]; - const size_t d = v.size(); - rhs.segment(start, d) = v; - start += d; - } - - // apply parents to rhs - for (const_iterator parent = beginParents(); parent != endParents(); ++parent) { - rhs += -get_S(parent) * x[*parent]; - } - - // solve system - backsubstitution - Vector soln = permutation_.transpose() * get_R().triangularView().solve(rhs); - - // apply solution: inlined manually due to permutation - size_t solnStart = 0; - for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - const size_t d = this->dim(frontal); - x[*frontal] = soln.segment(solnStart, d); - solnStart += d; - } -} - -/* ************************************************************************* */ -VectorValues GaussianConditional::solve(const VectorValues& x) const { - VectorValues result = x; - solveInPlace(result); - return result; + doSolveInPlace(*this, x); // Call helper version above } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index f2a77c894..b33098061 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -43,7 +43,7 @@ class JacobianFactor; /** * A conditional Gaussian functions as the node in a Bayes network * It has a set of parents y,z, etc. and implements a probability density on x. - * The negative log-probability is given by \f$ |Rx - (d - Sy - Tz - ...)|^2 \f$ + * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ */ class GaussianConditional : public IndexConditional { @@ -137,6 +137,15 @@ public: /** Copy constructor */ GaussianConditional(const GaussianConditional& rhs); + /** Combine several GaussianConditional into a single dense GC. The + * conditionals enumerated by \c first and \c last must be in increasing + * order, meaning that the parents of any conditional may not include a + * conditional coming before it. + * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. + * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. */ + template + static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); + /** Assignment operator */ GaussianConditional& operator=(const GaussianConditional& rhs); @@ -188,46 +197,39 @@ public: */ boost::shared_ptr toFactor() const; - /** - * Adds the RHS to a given VectorValues for use in solve() functions. - * @param x is the values to be updated, assumed allocated - */ - void rhs(VectorValues& x) const; - /** - * Adds the RHS to a given VectorValues for use in solve() functions. - * @param x is the values to be updated, assumed allocated - */ - void rhs(Permuted& x) const; - - /** - * solves a conditional Gaussian and stores the result in x + * Solves a conditional Gaussian and writes the solution into the entries of + * \c x for each frontal variable of the conditional. The parents are + * assumed to have already been solved in and their values are read from \c x. * This function works for multiple frontal variables. - * NOTE: assumes that the RHS for the frontals is stored in x, and - * then replaces the RHS with the partial result for this conditional, - * assuming that parents have been solved already. * - * @param x values structure with solved parents, and the RHS for this conditional - * @return solution \f$ x = R \ (d - Sy - Tz - ...) \f$ for each frontal variable + * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, + * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator + * variables of this conditional, this solve function computes + * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. + * + * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the + * solution \f$ x_f \f$ will be written. */ void solveInPlace(VectorValues& x) const; /** - * solves a conditional Gaussian and stores the result in x - * Identical to solveInPlace() above, with a permuted x + * Solves a conditional Gaussian and writes the solution into the entries of + * \c x for each frontal variable of the conditional (version for permuted + * VectorValues). The parents are assumed to have already been solved in + * and their values are read from \c x. This function works for multiple + * frontal variables. + * + * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, + * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator + * variables of this conditional, this solve function computes + * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. + * + * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the + * solution \f$ x_f \f$ will be written. */ void solveInPlace(Permuted& x) const; - /** - * Solves a conditional Gaussian and returns a new VectorValues - * This function works for multiple frontal variables, but should - * only be used for testing as it copies the input vector values - * - * Assumes, as in solveInPlace, that the RHS has been stored in x - * for all frontal variables - */ - VectorValues solve(const VectorValues& x) const; - // functions for transpose backsubstitution /** @@ -274,5 +276,49 @@ GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, } /* ************************************************************************* */ +template +GaussianConditional::shared_ptr GaussianConditional::Combine(ITERATOR firstConditional, ITERATOR lastConditional) { + + // TODO: check for being a clique + + // Get dimensions from first conditional + std::vector dims; dims.reserve((*firstConditional)->size() + 1); + for(const_iterator j = (*firstConditional)->begin(); j != (*firstConditional)->end(); ++j) + dims.push_back((*firstConditional)->dim(j)); + dims.push_back(1); + + // We assume the conditionals form clique, so the first n variables will be + // frontal variables in the new conditional. + size_t nFrontals = 0; + size_t nRows = 0; + for(ITERATOR c = firstConditional; c != lastConditional; ++c) { + nRows += dims[nFrontals]; + ++ nFrontals; + } + + // Allocate combined conditional, has same keys as firstConditional + Matrix tempCombined; + VerticalBlockView tempBlockView(tempCombined, dims.begin(), dims.end(), 0); + GaussianConditional::shared_ptr combinedConditional(new GaussianConditional((*firstConditional)->begin(), (*firstConditional)->end(), nFrontals, tempBlockView, zero(nRows))); + + // Resize to correct number of rows + combinedConditional->matrix_.resize(nRows, combinedConditional->matrix_.cols()); + combinedConditional->rsd_.rowEnd() = combinedConditional->matrix_.rows(); + + // Copy matrix and sigmas + const size_t totalDims = combinedConditional->matrix_.cols(); + size_t currentSlot = 0; + for(ITERATOR c = firstConditional; c != lastConditional; ++c) { + const size_t startRow = combinedConditional->rsd_.offset(currentSlot); // Start row is same as start column + combinedConditional->rsd_.range(0, currentSlot).block(startRow, 0, dims[currentSlot], combinedConditional->rsd_.offset(currentSlot)).operator=( + Matrix::Zero(dims[currentSlot], combinedConditional->rsd_.offset(currentSlot))); + combinedConditional->rsd_.range(currentSlot, dims.size()).block(startRow, 0, dims[currentSlot], totalDims - startRow).operator=( + (*c)->matrix_); + combinedConditional->sigmas_.segment(startRow, dims[currentSlot]) = (*c)->sigmas_; + ++ currentSlot; + } + + return combinedConditional; +} } // gtsam diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 72e3bb325..5300d594f 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -42,7 +42,6 @@ namespace gtsam { Index k = firstFrontalKey(); // a VectorValues that only has a value for k: cannot be printed if k<>0 x.insert(k, Vector(sigmas_.size())); - rhs(x); solveInPlace(x); return x[k]; } diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp index a9aa2479c..4402d8b05 100644 --- a/gtsam/linear/GaussianISAM.cpp +++ b/gtsam/linear/GaussianISAM.cpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -50,50 +51,14 @@ Matrix GaussianISAM::marginalCovariance(Index j) const { return Super::jointBayesNet(key1, key2, &EliminateQR); } -/* ************************************************************************* */ -void optimize(const BayesTree::sharedClique& clique, VectorValues& result) { - // parents are assumed to already be solved and available in result - // RHS for current conditional should already be in place in result - clique->conditional()->solveInPlace(result); - - BOOST_FOREACH(const BayesTree::sharedClique& child, clique->children_) - optimize(child, result); -} - -/* ************************************************************************* */ -void treeRHS(const BayesTree::sharedClique& clique, VectorValues& result) { - clique->conditional()->rhs(result); - BOOST_FOREACH(const BayesTree::sharedClique& child, clique->children_) - treeRHS(child, result); -} - -/* ************************************************************************* */ -VectorValues rhs(const BayesTree& bayesTree, boost::optional dims) { - VectorValues result; - if(dims) - result = VectorValues(*dims); - else - result = *allocateVectorValues(bayesTree); // allocate - treeRHS(bayesTree.root(), result); // recursively fill - return result; -} - /* ************************************************************************* */ VectorValues optimize(const GaussianISAM& isam) { - VectorValues result = rhs(isam, isam.dims_); - // starting from the root, call optimize on each conditional - optimize(isam.root(), result); + VectorValues result(isam.dims_); + // Call optimize for BayesTree + optimizeInPlace((const BayesTree&)isam, result); return result; } -/* ************************************************************************* */ -VectorValues optimize(const BayesTree& bayesTree) { - VectorValues result = rhs(bayesTree); - // starting from the root, call optimize on each conditional - optimize(bayesTree.root(), result); - return result; -} - /* ************************************************************************* */ BayesNet GaussianISAM::shortcut(sharedClique clique, sharedClique root) { return clique->shortcut(root,&EliminateQR); diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index a06c50657..af96d5317 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -90,19 +90,7 @@ public: }; // \class GaussianISAM -/** load a VectorValues with the RHS of the system for backsubstitution */ -VectorValues rhs(const BayesTree& bayesTree, boost::optional dims = boost::none); - -/** recursively load RHS for system */ -void treeRHS(const BayesTree::sharedClique& clique, VectorValues& result); - -// recursively optimize this conditional and all subtrees -void optimize(const BayesTree::sharedClique& clique, VectorValues& result); - // optimize the BayesTree, starting from the root VectorValues optimize(const GaussianISAM& isam); -// optimize the BayesTree, starting from the root -VectorValues optimize(const BayesTree& bayesTree); - } // \namespace gtsam diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index e2e1c2951..2c2a44dbb 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include @@ -33,32 +34,6 @@ namespace gtsam { using namespace std; - /* ************************************************************************* */ - void GaussianJunctionTree::btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const { - // solve the bayes net in the current node - current->conditional()->solveInPlace(config); - - // GaussianBayesNet::const_reverse_iterator it = current->rbegin(); -// for (; it!=current->rend(); ++it) { -// (*it)->solveInPlace(config); // solve and store result -// -//// Vector x = (*it)->solve(config); // Solve for that variable -//// config[(*it)->key()] = x; // store result in partial solution -// } - - // solve the bayes nets in the child nodes - BOOST_FOREACH(const BTClique::shared_ptr& child, current->children()) { - btreeBackSubstitute(child, config); - } - } - - /* ************************************************************************* */ - void GaussianJunctionTree::btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const { - current->conditional()->rhs(config); - BOOST_FOREACH(const BTClique::shared_ptr& child, current->children()) - btreeRHS(child, config); - } - /* ************************************************************************* */ VectorValues GaussianJunctionTree::optimize(Eliminate function) const { tic(1, "GJT eliminate"); @@ -71,12 +46,11 @@ namespace gtsam { vector dims(rootClique->conditional()->back()+1, 0); countDims(rootClique, dims); VectorValues result(dims); - btreeRHS(rootClique, result); toc(2, "allocate VectorValues"); // back-substitution tic(3, "back-substitute"); - btreeBackSubstitute(rootClique, result); + internal::optimizeInPlace(rootClique, result); toc(3, "back-substitute"); return result; } diff --git a/gtsam/linear/GaussianJunctionTree.h b/gtsam/linear/GaussianJunctionTree.h index be98669f1..31a0a1680 100644 --- a/gtsam/linear/GaussianJunctionTree.h +++ b/gtsam/linear/GaussianJunctionTree.h @@ -45,13 +45,6 @@ namespace gtsam { typedef Base::sharedClique sharedClique; typedef GaussianFactorGraph::Eliminate Eliminate; - protected: - // back-substitute in topological sort order (parents first) - void btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const; - - // find the RHS for the system in order to perform backsubstitution - void btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const; - public : /** Default constructor */ diff --git a/gtsam/linear/GaussianSequentialSolver.cpp b/gtsam/linear/GaussianSequentialSolver.cpp index bfff98e0f..42bce03ce 100644 --- a/gtsam/linear/GaussianSequentialSolver.cpp +++ b/gtsam/linear/GaussianSequentialSolver.cpp @@ -78,7 +78,8 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const { tic(2,"optimize"); // Back-substitute - VectorValues::shared_ptr solution(gtsam::optimize_(*bayesNet)); + VectorValues::shared_ptr solution( + new VectorValues(gtsam::optimize(*bayesNet))); toc(2,"optimize"); if(debug) solution->print("GaussianSequentialSolver, solution "); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 2a16c0e38..c1e5f016a 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -71,7 +71,7 @@ namespace gtsam { * and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$ * to arrive at the canonical form of the Gaussian: * @f[ - * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu + C + * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu * @f] * * This factor is one of the factors that can be in a GaussianFactorGraph. diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 0218ecc8d..dbc2a5cff 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -642,8 +642,9 @@ namespace gtsam { r.vector() = Vector::Zero(r.dim()); Index i = 0; BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { + SubVector &y = r[i]; for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - r[i] += factor->getA(j) * x[*j]; + y += factor->getA(j) * x[*j]; } ++i; } diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 59fd824bf..02048d3df 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -82,7 +82,7 @@ namespace gtsam { /* ************************************************************************* */ // take a step, return true if converged bool step(const S& Ab, V& x) { - if ((++k) >= parameters_.maxIterations()) return true; + if ((++k) >= ((int)parameters_.maxIterations())) return true; //----------------------------------> double alpha = takeOptimalStep(x); diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 9bac18da0..b19a1fda5 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -144,48 +144,6 @@ TEST( GaussianConditional, equals ) EXPECT( expected.equals(actual) ); } -/* ************************************************************************* */ -TEST( GaussianConditional, rhs_permuted ) -{ - // Test filling the rhs when the VectorValues is permuted - - // Create a VectorValues - VectorValues unpermuted(5, 2); - unpermuted[0] << 1, 2; - unpermuted[1] << 3, 4; - unpermuted[2] << 5, 6; - unpermuted[3] << 7, 8; - unpermuted[4] << 9, 10; - - // Create a permutation - Permutation permutation(5); - permutation[0] = 4; - permutation[1] = 3; - permutation[2] = 2; - permutation[3] = 1; - permutation[4] = 0; - - // Permuted VectorValues - Permuted permuted(permutation, unpermuted); - - // Expected VectorValues - VectorValues expected(5, 2); - expected[0] << 1, 2; - expected[1] << 3, 4; - expected[2] << 5, 6; - expected[3] << 7, 8; - expected[4] << 11, 12; - - // GaussianConditional - Vector d(2); d << 11, 12; - GaussianConditional conditional(0, d, Matrix::Identity(2,2), Vector::Ones(2)); - - // Fill rhs, conditional is on index 0, which should fill slot 4 of the values - conditional.rhs(permuted); - - EXPECT(assert_equal(expected, unpermuted)); -} - /* ************************************************************************* */ TEST( GaussianConditional, solve ) { @@ -208,8 +166,7 @@ TEST( GaussianConditional, solve ) Vector tau = ones(2); - // RHS is different than the one in the solution vector - GaussianConditional cg(_x_,ones(2), R, _x1_, A1, _l1_, A2, tau); + GaussianConditional cg(_x_, d, R, _x1_, A1, _l1_, A2, tau); Vector sx1(2); sx1(0) = 1.0; sx1(1) = 1.0; @@ -218,21 +175,16 @@ TEST( GaussianConditional, solve ) sl1(0) = 1.0; sl1(1) = 1.0; VectorValues solution(vector(3, 2)); - solution[_x_] = d; // RHS + solution[_x_] = d; solution[_x1_] = sx1; // parents solution[_l1_] = sl1; - // NOTE: the solve functions assume the RHS is passed as the initialization of - // the solution. VectorValues expected(vector(3, 2)); expected[_x_] = expectedX; expected[_x1_] = sx1; expected[_l1_] = sl1; - - VectorValues copy_result = cg.solve(solution); cg.solveInPlace(solution); - EXPECT(assert_equal(expected, copy_result, 0.0001)); EXPECT(assert_equal(expected, solution, 0.0001)); } @@ -240,12 +192,11 @@ TEST( GaussianConditional, solve ) TEST( GaussianConditional, solve_simple ) { // no pivoting from LDL, so R matrix is not permuted - // RHS is deliberately not the same as below Matrix full_matrix = Matrix_(4, 7, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.0); + 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, + 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, + 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, + 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); // solve system as a non-multifrontal version first // 2 variables, frontal has dim=4 @@ -261,7 +212,6 @@ TEST( GaussianConditional, solve_simple ) // elimination order; _x_, _x1_ vector vdim; vdim += 4, 2; VectorValues actual(vdim); - actual[_x_] = Vector_(4, 0.1, 0.2, 0.3, 0.4); // d actual[_x1_] = sx1; // parent VectorValues expected(vdim); @@ -283,10 +233,10 @@ TEST( GaussianConditional, solve_multifrontal ) // create full system, 3 variables, 2 frontals, all 2 dim // no pivoting from LDL, so R matrix is not permuted Matrix full_matrix = Matrix_(4, 7, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.5, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.6, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.7, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.8); + 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, + 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, + 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, + 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); // 3 variables, all dim=2 vector dims; dims += 2, 2, 2, 1; @@ -295,15 +245,13 @@ TEST( GaussianConditional, solve_multifrontal ) vector cgdims; cgdims += _x_, _x1_, _l1_; GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas); - EXPECT(assert_equal(Vector_(4, 0.5, 0.6, 0.7, 0.8), cg.get_d())); + EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d())); // partial solution Vector sl1 = Vector_(2, 9.0, 10.0); // elimination order; _x_, _x1_, _l1_ VectorValues actual(vector(3, 2)); - actual[_x_] = Vector_(2, 0.1, 0.2); // rhs - actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs actual[_l1_] = sl1; // parent VectorValues expected(vector(3, 2)); @@ -327,10 +275,10 @@ TEST( GaussianConditional, solve_multifrontal_permuted ) // create full system, 3 variables, 2 frontals, all 2 dim // no pivoting from LDL, so R matrix is not permuted Matrix full_matrix = Matrix_(4, 7, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.5, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.6, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.7, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.8); + 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, + 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, + 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, + 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); // 3 variables, all dim=2 vector dims; dims += 2, 2, 2, 1; @@ -339,7 +287,7 @@ TEST( GaussianConditional, solve_multifrontal_permuted ) vector cgdims; cgdims += _x_, _x1_, _l1_; GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas); - EXPECT(assert_equal(Vector_(4, 0.5, 0.6, 0.7, 0.8), cg.get_d())); + EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d())); // partial solution Vector sl1 = Vector_(2, 9.0, 10.0); diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp index 2462f5255..3a9bcb4ad 100644 --- a/gtsam/linear/tests/testGaussianJunctionTree.cpp +++ b/gtsam/linear/tests/testGaussianJunctionTree.cpp @@ -103,6 +103,21 @@ TEST( GaussianJunctionTree, eliminate ) EXPECT(assert_equal(*(bayesTree_expected.root()->children().front()), *(rootClique->children().front()))); } +/* ************************************************************************* */ +TEST_UNSAFE( GaussianJunctionTree, GBNConstructor ) +{ + GaussianFactorGraph fg = createChain(); + GaussianJunctionTree jt(fg); + BayesTree::sharedClique root = jt.eliminate(&EliminateQR); + BayesTree expected; + expected.insert(root); + + GaussianBayesNet bn(*GaussianSequentialSolver(fg).eliminate()); + BayesTree actual(bn); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST( GaussianJunctionTree, optimizeMultiFrontal ) { diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index af1e345b6..5feaa401a 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -115,29 +115,6 @@ struct DoglegOptimizerImpl { */ static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false); - /** Compute the minimizer \f$ \delta x_u \f$ of the line search along the gradient direction \f$ g \f$ of - * the function - * \f[ - * M(\delta x) = (R \delta x - d)^T (R \delta x - d) - * \f] - * where \f$ R \f$ is an upper-triangular matrix and \f$ d \f$ is a vector. - * Together \f$ (R,d) \f$ are either a Bayes' net or a Bayes' tree. - * - * The same quadratic error function written as a Taylor expansion of the original - * non-linear error function is - * \f[ - * M(\delta x) = f(x_0) + g(x_0) + \frac{1}{2} \delta x^T G(x_0) \delta x, - * \f] - * @tparam M The type of the Bayes' net or tree, currently - * either BayesNet (or GaussianBayesNet) or BayesTree. - * @param Rd The Bayes' net or tree \f$ (R,d) \f$ as described above, currently - * this must be of type BayesNet (or GaussianBayesNet) or - * BayesTree. - * @return The minimizer \f$ \delta x_u \f$ along the gradient descent direction. - */ - template - static VectorValues ComputeSteepestDescentPoint(const M& Rd); - /** Compute the point on the line between the steepest descent point and the * Newton's method point intersecting the trust region boundary. * Mathematically, computes \f$ \tau \f$ such that \f$ 0<\tau<1 \f$ and @@ -159,7 +136,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( // Compute steepest descent and Newton's method points tic(0, "Steepest Descent"); - VectorValues dx_u = ComputeSteepestDescentPoint(Rd); + VectorValues dx_u = optimizeGradientSearch(Rd); toc(0, "Steepest Descent"); tic(1, "optimize"); VectorValues dx_n = optimize(Rd); @@ -173,7 +150,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( IterationResult result; bool stay = true; - enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction; // Used to prevent alternating between increasing and decreasing in one iteration + enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction = NONE; // Used to prevent alternating between increasing and decreasing in one iteration while(stay) { tic(3, "Dog leg point"); // Compute dog leg point @@ -274,33 +251,4 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( return result; } -/* ************************************************************************* */ -template -VectorValues DoglegOptimizerImpl::ComputeSteepestDescentPoint(const M& Rd) { - - tic(0, "Compute Gradient"); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - VectorValues grad = *allocateVectorValues(Rd); - gradientAtZero(Rd, grad); - double gradientSqNorm = grad.dot(grad); - toc(0, "Compute Gradient"); - - tic(1, "Compute R*g"); - // Compute R * g - FactorGraph Rd_jfg(Rd); - Errors Rg = Rd_jfg * grad; - toc(1, "Compute R*g"); - - tic(2, "Compute minimizing step size"); - // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - toc(2, "Compute minimizing step size"); - - tic(3, "Compute point"); - // Compute steepest descent point - scal(step, grad); - toc(3, "Compute point"); - return grad; -} - } diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h index de3992d28..57801fbe4 100644 --- a/gtsam/nonlinear/GaussianISAM2-inl.h +++ b/gtsam/nonlinear/GaussianISAM2-inl.h @@ -18,16 +18,16 @@ #include #include -using namespace std; -using namespace gtsam; - #include namespace gtsam { + using namespace std; + /* ************************************************************************* */ + namespace internal { template - void optimize2(const boost::shared_ptr& clique, double threshold, + void optimizeWildfire(const boost::shared_ptr& clique, double threshold, vector& changed, const vector& replaced, Permuted& delta, int& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the @@ -64,7 +64,6 @@ namespace gtsam { } // Back-substitute - (*clique)->rhs(delta); (*clique)->solveInPlace(delta); count += (*clique)->nrFrontals(); @@ -98,45 +97,61 @@ namespace gtsam { // Recurse to children BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - optimize2(child, threshold, changed, replaced, delta, count); + optimizeWildfire(child, threshold, changed, replaced, delta, count); } } } - - /* ************************************************************************* */ - // fast full version without threshold - template - void optimize2(const boost::shared_ptr& clique, VectorValues& delta) { - - // parents are assumed to already be solved and available in result - (*clique)->rhs(delta); - (*clique)->solveInPlace(delta); - - // Solve chilren recursively - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - optimize2(child, delta); - } } - ///* ************************************************************************* */ - //boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { - // boost::shared_ptr delta(new VectorValues()); - // set changed; - // // starting from the root, call optimize on each conditional - // optimize2(root, delta); - // return delta; - //} - /* ************************************************************************* */ template - int optimize2(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { + int optimizeWildfire(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { vector changed(keys.size(), false); int count = 0; // starting from the root, call optimize on each conditional - optimize2(root, threshold, changed, keys, delta, count); + if(root) + internal::optimizeWildfire(root, threshold, changed, keys, delta, count); return count; } + /* ************************************************************************* */ + template + VectorValues optimizeGradientSearch(const ISAM2& isam) { + tic(0, "Allocate VectorValues"); + VectorValues grad = *allocateVectorValues(isam); + toc(0, "Allocate VectorValues"); + + optimizeGradientSearchInPlace(isam, grad); + + return grad; + } + + /* ************************************************************************* */ + template + void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { + tic(1, "Compute Gradient"); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(Rd, grad); + double gradientSqNorm = grad.dot(grad); + toc(1, "Compute Gradient"); + + tic(2, "Compute R*g"); + // Compute R * g + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; + toc(2, "Compute R*g"); + + tic(3, "Compute minimizing step size"); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + toc(3, "Compute minimizing step size"); + + tic(4, "Compute point"); + // Compute steepest descent point + scal(step, grad); + toc(4, "Compute point"); + } + /* ************************************************************************* */ template void nnz_internal(const boost::shared_ptr& clique, int& result) { diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp index 34456ad36..624c5d4f0 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -26,16 +26,6 @@ using namespace gtsam; namespace gtsam { - /* ************************************************************************* */ - VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0) { - return gradient(FactorGraph(bayesTree), x0); - } - - /* ************************************************************************* */ - void gradientAtZero(const BayesTree& bayesTree, VectorValues& g) { - gradientAtZero(FactorGraph(bayesTree), g); - } - /* ************************************************************************* */ VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0) { return gradient(FactorGraph(bayesTree), x0); diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index d1a4bef9f..a602d81a6 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -63,48 +63,65 @@ public: }; -/** optimize the BayesTree, starting from the root */ -template -void optimize2(const boost::shared_ptr& root, VectorValues& delta); +/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ +template +VectorValues optimize(const ISAM2& isam) { + VectorValues delta = *allocateVectorValues(isam); + internal::optimizeInPlace(isam.root(), delta); + return delta; +} -/// optimize the BayesTree, starting from the root; "replaced" needs to contain +/// Optimize the BayesTree, starting from the root. +/// @param replaced Needs to contain /// all variables that are contained in the top of the Bayes tree that has been -/// redone; "delta" is the current solution, an offset from the linearization -/// point; "threshold" is the maximum change against the PREVIOUS delta for +/// redone. +/// @param delta The current solution, an offset from the linearization +/// point. +/// @param threshold The maximum change against the PREVIOUS delta for /// non-replaced variables that can be ignored, ie. the old delta entry is kept /// and recursive backsubstitution might eventually stop if none of the changed /// variables are contained in the subtree. -/// returns the number of variables that were solved for +/// @return The number of variables that were solved for template -int optimize2(const boost::shared_ptr& root, +int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& replaced, Permuted& delta); +/** + * Optimize along the gradient direction, with a closed-form computation to + * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized + * problem. The error function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the + * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size + * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields + * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function + * evaluates the denominator without computing the Hessian \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] + */ +template +VectorValues optimizeGradientSearch(const ISAM2& isam); + +/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ +template +void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad); + /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) template int calculate_nnz(const boost::shared_ptr& clique); -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ R^T(Rx-d) \f$. - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ -VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0); - -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around zero. - * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ -void gradientAtZero(const BayesTree& bayesTree, VectorValues& g); - /** * Compute the gradient of the energy function, * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index c7224884e..40e87a09b 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -15,6 +15,8 @@ * @author Michael Kaess, Richard Roberts */ +#include + namespace gtsam { using namespace std; @@ -46,8 +48,8 @@ struct ISAM2::Impl { * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, - typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol @@ -121,12 +123,15 @@ struct ISAM2::Impl { static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode); + static size_t UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + }; /* ************************************************************************* */ template void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { + const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -153,6 +158,7 @@ void ISAM2::Impl::AddVariables( assert(ordering.size() == delta.size()); } assert(ordering.nVars() >= nodes.size()); + replacedKeys.resize(ordering.nVars(), false); nodes.resize(ordering.nVars()); } @@ -229,7 +235,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permute invalidateIfDebug = boost::optional&>(); #endif - assert(values.size() == ordering.size()); + assert(values.size() == ordering.nVars()); + assert(delta.size() == ordering.nVars()); Values::iterator key_value; Ordering::const_iterator key_index; for(key_value = values.begin(), key_index = ordering.begin(); @@ -303,7 +310,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, } } } - Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD_(affectedFactorsIndex, cmember)); + Permutation::shared_ptr affectedColamd(inference::PermutationCOLAMD_(affectedFactorsIndex, cmember)); toc(3,"ccolamd"); tic(4,"ccolamd permutations"); Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse()); @@ -354,4 +361,55 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, return result; } +/* ************************************************************************* */ +namespace internal { +inline static void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result) { + // parents are assumed to already be solved and available in result + clique->conditional()->solveInPlace(result); + + // starting from the root, call optimize on each conditional + BOOST_FOREACH(const boost::shared_ptr >& child, clique->children_) + optimizeInPlace(child, result); +} +} + +/* ************************************************************************* */ +template +size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { + + size_t lastBacksubVariableCount; + + if (wildfireThreshold <= 0.0) { + // Threshold is zero or less, so do a full recalculation + // Collect dimensions and allocate new VectorValues + vector dims(delta.size()); + for(size_t j=0; jdim(j); + VectorValues newDelta(dims); + + // Optimize full solution delta + internal::optimizeInPlace(root, newDelta); + + // Copy solution into delta + delta.permutation() = Permutation::Identity(delta.size()); + delta.container() = newDelta; + + lastBacksubVariableCount = delta.size(); + + } else { + // Optimize with wildfire + lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_ + +#ifndef NDEBUG + for(size_t j=0; j).all()); +#endif + } + + // Clear replacedKeys + replacedKeys.assign(replacedKeys.size(), false); + + return lastBacksubVariableCount; +} + } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 6ff22c3a1..0d5f190a3 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -41,7 +41,7 @@ static const double batchThreshold = 0.65; /* ************************************************************************* */ template ISAM2::ISAM2(const ISAM2Params& params): - delta_(Permutation(), deltaUnpermuted_), params_(params) { + delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -50,7 +50,7 @@ ISAM2::ISAM2(const ISAM2Params& params): /* ************************************************************************* */ template ISAM2::ISAM2(): - delta_(Permutation(), deltaUnpermuted_) { + delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -238,7 +238,7 @@ boost::shared_ptr > ISAM2::recalculate( if(theta_.size() > constrainedKeysSet.size()) { BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; } } - Permutation::shared_ptr colamd(Inference::PermutationCOLAMD_(variableIndex_, cmember)); + Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); Permutation::shared_ptr colamdInverse(colamd->inverse()); toc(1,"CCOLAMD"); @@ -413,13 +413,21 @@ ISAM2Result ISAM2::update( lastBacksubVariableCount = 0; lastNnzTop = 0; ISAM2Result result; + const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); if(verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); } - tic(0,"push_back factors"); + // Update delta if we need it to check relinearization later + if(relinearizeThisStep) { + tic(0, "updateDelta"); + updateDelta(disableReordering); + toc(0, "updateDelta"); + } + + tic(1,"push_back factors"); // Add the new factor indices to the result struct result.newFactorsIndices.resize(newFactors.size()); for(size_t i=0; i::update( // Remove removed factors from the variable index so we do not attempt to relinearize them variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_)); - toc(0,"push_back factors"); + toc(1,"push_back factors"); - tic(1,"add new variables"); + tic(2,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, ordering_, Base::nodes_); - toc(1,"add new variables"); + Impl::AddVariables(newTheta, theta_, delta_, deltaReplacedMask_, ordering_, Base::nodes_); + toc(2,"add new variables"); - tic(2,"evaluate error before"); + tic(3,"evaluate error before"); if(params_.evaluateNonlinearError) result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); - toc(2,"evaluate error before"); + toc(3,"evaluate error before"); - tic(3,"gather involved keys"); + tic(4,"gather involved keys"); // 3. Mark linear update FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors // Also mark keys involved in removed factors @@ -462,11 +470,11 @@ ISAM2Result ISAM2::update( // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Index value) instead of the iterator constructor. FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them - toc(3,"gather involved keys"); + toc(4,"gather involved keys"); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold - if (force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0)) { // todo: every n steps - tic(4,"gather relinearize keys"); + if (relinearizeThisStep) { + tic(5,"gather relinearize keys"); vector markedRelinMask(ordering_.nVars(), false); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. FastSet relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); @@ -475,19 +483,19 @@ ISAM2Result ISAM2::update( // Add the variables being relinearized to the marked keys BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } markedKeys.insert(relinKeys.begin(), relinKeys.end()); - toc(4,"gather relinearize keys"); + toc(5,"gather relinearize keys"); - tic(5,"fluid find_all"); + tic(6,"fluid find_all"); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. if (!relinKeys.empty() && this->root()) Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator - toc(5,"fluid find_all"); + toc(6,"fluid find_all"); - tic(6,"expmap"); + tic(7,"expmap"); // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. if (!relinKeys.empty()) Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); - toc(6,"expmap"); + toc(7,"expmap"); result.variablesRelinearized = markedKeys.size(); @@ -501,7 +509,7 @@ ISAM2Result ISAM2::update( #endif } - tic(7,"linearize new"); + tic(8,"linearize new"); tic(1,"linearize"); // 7. Linearize new factors FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); @@ -511,9 +519,9 @@ ISAM2Result ISAM2::update( // Augment the variable index with the new factors variableIndex_.augment(*linearFactors); toc(2,"augment VI"); - toc(7,"linearize new"); + toc(8,"linearize new"); - tic(8,"recalculate"); + tic(9,"recalculate"); // 8. Redo top of Bayes tree // Convert constrained symbols to indices boost::optional > constrainedIndices; @@ -526,49 +534,17 @@ ISAM2Result ISAM2::update( boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !newKeys.empty()) replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); - toc(8,"recalculate"); - tic(9,"solve"); + // Update replaced keys mask (accumulates until back-substitution takes place) + if(replacedKeys) { + BOOST_FOREACH(const Index var, *replacedKeys) { + deltaReplacedMask_[var] = true; } } + toc(9,"recalculate"); + + //tic(9,"solve"); // 9. Solve - if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { - // See note in gtsam/base/boost_variant_with_workaround.h - const ISAM2GaussNewtonParams& gaussNewtonParams = - boost::get(params_.optimizationParams); - if (gaussNewtonParams.wildfireThreshold <= 0.0 || disableReordering) { - VectorValues newDelta(theta_.dims(ordering_)); - optimize2(this->root(), newDelta); - if(debug) newDelta.print("newDelta: "); - assert(newDelta.size() == delta_.size()); - delta_.permutation() = Permutation::Identity(delta_.size()); - delta_.container() = newDelta; - lastBacksubVariableCount = theta_.size(); - } else { - vector replacedKeysMask(variableIndex_.size(), false); - if(replacedKeys) { - BOOST_FOREACH(const Index var, *replacedKeys) { - replacedKeysMask[var] = true; } } - lastBacksubVariableCount = optimize2(this->root(), gaussNewtonParams.wildfireThreshold, replacedKeysMask, delta_); // modifies delta_ - -#ifndef NDEBUG - for(size_t j=0; j).all()); -#endif - } - } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { - // See note in gtsam/base/boost_variant_with_workaround.h - const ISAM2DoglegParams& doglegParams = - boost::get(params_.optimizationParams); - // Do one Dogleg iteration - tic(1, "Dogleg Iterate"); - DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); - toc(1, "Dogleg Iterate"); - // Update Delta and linear step - doglegDelta_ = doglegResult.Delta; - delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation - delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution - } - toc(9,"solve"); + if(debug) delta_.print("delta_: "); + //toc(9,"solve"); tic(10,"evaluate error after"); if(params_.evaluateNonlinearError) @@ -576,10 +552,47 @@ ISAM2Result ISAM2::update( toc(10,"evaluate error after"); result.cliques = this->nodes().size(); + deltaUptodate_ = false; return result; } +/* ************************************************************************* */ +template +void ISAM2::updateDelta(bool forceFullSolve) const { + + if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + // If using Gauss-Newton, update with wildfireThreshold + const ISAM2GaussNewtonParams& gaussNewtonParams = + boost::get(params_.optimizationParams); + const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; + tic(0, "Wildfire update"); + lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold); + toc(0, "Wildfire update"); + + } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + // If using Dogleg, do a Dogleg step + const ISAM2DoglegParams& doglegParams = + boost::get(params_.optimizationParams); + + // Do one Dogleg iteration + tic(1, "Dogleg Iterate"); + DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); + toc(1, "Dogleg Iterate"); + + // Update Delta and linear step + doglegDelta_ = doglegResult.Delta; + delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation + delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + + // Clear replaced mask + deltaReplacedMask_.assign(deltaReplacedMask_.size(), false); + } + + deltaUptodate_ = true; +} + /* ************************************************************************* */ template Values ISAM2::calculateEstimate() const { @@ -587,17 +600,17 @@ Values ISAM2::calculateEstimate() const { // handles Permuted Values ret(theta_); vector mask(ordering_.nVars(), true); - Impl::ExpmapMasked(ret, delta_, ordering_, mask); + Impl::ExpmapMasked(ret, getDelta(), ordering_, mask); return ret; } /* ************************************************************************* */ template -template -typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { +template +VALUE ISAM2::calculateEstimate(Key key) const { const Index index = getOrdering()[key]; const SubVector delta = getDelta()[index]; - return getLinearizationPoint()[key].retract(delta); + return theta_.at(key).retract(delta); } /* ************************************************************************* */ @@ -610,10 +623,10 @@ Values ISAM2::calculateBestEstimate() const { /* ************************************************************************* */ template -VectorValues optimize(const ISAM2& isam) { - VectorValues delta = *allocateVectorValues(isam); - optimize2(isam.root(), delta); - return delta; +const Permuted& ISAM2::getDelta() const { + if(!deltaUptodate_) + updateDelta(); + return delta_; } } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 4c8df2997..208f0748f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -283,19 +283,42 @@ protected: /** The linear delta from the last linear solution, an update to the estimate in theta */ VectorValues deltaUnpermuted_; - /** @brief The permutation through which the deltaUnpermuted_ is + /** The permutation through which the deltaUnpermuted_ is * referenced. * * Permuting Vector entries would be slow, so for performance we * instead maintain this permutation through which we access the linear delta * indirectly + * + * This is \c mutable because it is a "cached" variable - it is not updated + * until either requested with getDelta() or calculateEstimate(), or needed + * during update() to evaluate whether to relinearize variables. */ - Permuted delta_; + mutable Permuted delta_; + + /** Indicates whether the current delta is up-to-date, only used + * internally - delta will always be updated if necessary when it is + * requested with getDelta() or calculateEstimate(). + * + * This is \c mutable because it is used internally to not update delta_ + * until it is needed. + */ + mutable bool deltaUptodate_; + + /** A cumulative mask for the variables that were replaced and have not yet + * been updated in the linear solution delta_, this is only used internally, + * delta will always be updated if necessary when requested with getDelta() + * or calculateEstimate(). + * + * This is \c mutable because it is used internally to not update delta_ + * until it is needed. + */ + mutable std::vector deltaReplacedMask_; /** All original nonlinear factors are stored here to use during relinearization */ GRAPH nonlinearFactors_; - /** @brief The current elimination ordering Symbols to Index (integer) keys. + /** The current elimination ordering Symbols to Index (integer) keys. * * We keep it up to date as we add and reorder variables. */ @@ -305,7 +328,7 @@ protected: ISAM2Params params_; /** The current Dogleg Delta (trust region radius) */ - boost::optional doglegDelta_; + mutable boost::optional doglegDelta_; private: #ifndef NDEBUG @@ -337,6 +360,8 @@ public: newISAM2->variableIndex_ = variableIndex_; newISAM2->deltaUnpermuted_ = deltaUnpermuted_; newISAM2->delta_ = delta_; + newISAM2->deltaUptodate_ = deltaUptodate_; + newISAM2->deltaReplacedMask_ = deltaReplacedMask_; newISAM2->nonlinearFactors_ = nonlinearFactors_; newISAM2->ordering_ = ordering_; newISAM2->params_ = params_; @@ -390,8 +415,8 @@ public: * @param key * @return */ - template - typename KEY::Value calculateEstimate(const KEY& key) const; + template + VALUE calculateEstimate(Key key) const; /// @name Public members for non-typical usage //@{ @@ -404,7 +429,7 @@ public: Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ - const Permuted& getDelta() const { return delta_; } + const Permuted& getDelta() const; /** Access the set of nonlinear factors */ const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; } @@ -416,7 +441,7 @@ public: size_t lastAffectedFactorCount; size_t lastAffectedCliqueCount; size_t lastAffectedMarkedCount; - size_t lastBacksubVariableCount; + mutable size_t lastBacksubVariableCount; size_t lastNnzTop; ISAM2Params params() const { return params_; } @@ -433,13 +458,10 @@ private: const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); + void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 -/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -template -VectorValues optimize(const ISAM2& isam); - } /// namespace gtsam #include diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index e260f24db..bf19555bf 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -66,13 +66,24 @@ public: } /** - * Constructor - * @param keys The variables involved in this factor + * Constructor from a vector of the keys involved in this factor + */ + NonlinearFactor(const std::vector& keys) : + Base(keys) {} + + /** + * Constructor from iterators over the keys involved in this factor */ template - NonlinearFactor(ITERATOR beginKeys, ITERATOR endKeys) { - this->keys_.insert(this->keys_.end(), beginKeys, endKeys); - } + NonlinearFactor(ITERATOR beginKeys, ITERATOR endKeys) : + Base(beginKeys, endKeys) {} + + NonlinearFactor(Key key) : Base(key) {} ///< Convenience constructor for 1 key + NonlinearFactor(Key key1, Key key2) : Base(key1, key2) {} ///< Convenience constructor for 2 keys + NonlinearFactor(Key key1, Key key2, Key key3) : Base(key1, key2, key3) {} ///< Convenience constructor for 3 keys + NonlinearFactor(Key key1, Key key2, Key key3, Key key4) : Base(key1, key2, key3, key4) {} ///< Convenience constructor for 4 keys + NonlinearFactor(Key key1, Key key2, Key key3, Key key4, Key key5) : Base(key1, key2, key3, key4, key5) {} ///< Convenience constructor for 5 keys + NonlinearFactor(Key key1, Key key2, Key key3, Key key4, Key key5, Key key6) : Base(key1, key2, key3, key4, key5, key6) {} ///< Convenience constructor for 6 keys /// @} /// @name Testable @@ -178,6 +189,13 @@ public: : Base(beginKeys, endKeys), noiseModel_(noiseModel) { } + NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key) : Base(key), noiseModel_(noiseModel) {} ///< Convenience constructor for 1 key + NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2) : Base(key1, key2), noiseModel_(noiseModel) {} ///< Convenience constructor for 2 keys + NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3) : Base(key1, key2, key3), noiseModel_(noiseModel) {} ///< Convenience constructor for 3 keys + NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4) : Base(key1, key2, key3, key4), noiseModel_(noiseModel) {} ///< Convenience constructor for 4 keys + NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4, Key key5) : Base(key1, key2, key3, key4, key5), noiseModel_(noiseModel) {} ///< Convenience constructor for 5 keys + NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4, Key key5, Key key6) : Base(key1, key2, key3, key4, key5, key6), noiseModel_(noiseModel) {} ///< Convenience constructor for 6 keys + protected: /** @@ -319,10 +337,7 @@ public: * @param key by which to look up X value in Values */ NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) : - Base(noiseModel) { - keys_.resize(1); - keys_[0] = key1; - } + Base(noiseModel, key1) {} /** Calls the 1-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ @@ -389,11 +404,7 @@ public: * @param j2 key of the second variable */ NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : - Base(noiseModel) { - keys_.resize(2); - keys_[0] = j1; - keys_[1] = j2; - } + Base(noiseModel, j1, j2) {} virtual ~NoiseModelFactor2() {} @@ -469,12 +480,7 @@ public: * @param j3 key of the third variable */ NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : - Base(noiseModel) { - keys_.resize(3); - keys_[0] = j1; - keys_[1] = j2; - keys_[2] = j3; - } + Base(noiseModel, j1, j2, j3) {} virtual ~NoiseModelFactor3() {} @@ -552,13 +558,7 @@ public: * @param j4 key of the fourth variable */ NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : - Base(noiseModel) { - keys_.resize(4); - keys_[0] = j1; - keys_[1] = j2; - keys_[2] = j3; - keys_[3] = j4; - } + Base(noiseModel, j1, j2, j3, j4) {} virtual ~NoiseModelFactor4() {} @@ -640,14 +640,7 @@ public: * @param j5 key of the fifth variable */ NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : - Base(noiseModel) { - keys_.resize(5); - keys_[0] = j1; - keys_[1] = j2; - keys_[2] = j3; - keys_[3] = j4; - keys_[4] = j5; - } + Base(noiseModel, j1, j2, j3, j4, j5) {} virtual ~NoiseModelFactor5() {} @@ -733,15 +726,7 @@ public: * @param j6 key of the fifth variable */ NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : - Base(noiseModel) { - keys_.resize(6); - keys_[0] = j1; - keys_[1] = j2; - keys_[2] = j3; - keys_[3] = j4; - keys_[4] = j5; - keys_[5] = j6; - } + Base(noiseModel, j1, j2, j3, j4, j5, j6) {} virtual ~NoiseModelFactor6() {} diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 647afa2d5..2400ea996 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -78,7 +78,7 @@ namespace gtsam { "orderingCOLAMD: some variables in the graph are not constrained!"); // Compute a fill-reducing ordering with COLAMD - Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD( + Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD( variableIndex)); // Permute the Ordering with the COLAMD ordering diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 10c3370cf..14be537d3 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -85,6 +85,14 @@ Index Ordering::pop_back(Key key) { } } +/* ************************************************************************* */ +Ordering::InvertedMap Ordering::invert() const { + InvertedMap result; + BOOST_FOREACH(const value_type& p, *this) + result.insert(make_pair(p.second, p.first)); + return result; +} + /* ************************************************************************* */ void Unordered::print(const string& s) const { cout << s << " (" << size() << "):"; diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index fcb3d5bfb..37f0fe106 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -40,6 +40,7 @@ protected: public: + typedef std::map InvertedMap; typedef boost::shared_ptr shared_ptr; typedef std::pair value_type; @@ -203,6 +204,12 @@ public: /// Synonym for operator[](Key) Index& at(Key key) { return operator[](key); } + /** + * Create an inverse mapping from Index->Key, useful for decoding linear systems + * @return inverse mapping structure + */ + InvertedMap invert() const; + /// @} /// @name Testable /// @{ diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index 945044492..f671a4b30 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -113,9 +113,15 @@ public: bool operator==(const Symbol& comp) const { return comp.c_ == c_ && comp.j_ == j_; } + bool operator==(Key comp) const { + return comp == (Key)(*this); + } bool operator!=(const Symbol& comp) const { return comp.c_ != c_ || comp.j_ != j_; } + bool operator!=(Key comp) const { + return comp != (Key)(*this); + } /** Return a filter function that returns true when evaluated on a Key whose * character (when converted to a Symbol) matches \c c. Use this with the @@ -138,5 +144,34 @@ private: } }; +namespace symbol_shorthand { +inline Key A(size_t j) { return Symbol('a', j); } +inline Key B(size_t j) { return Symbol('b', j); } +inline Key C(size_t j) { return Symbol('c', j); } +inline Key D(size_t j) { return Symbol('d', j); } +inline Key E(size_t j) { return Symbol('e', j); } +inline Key F(size_t j) { return Symbol('f', j); } +inline Key G(size_t j) { return Symbol('g', j); } +inline Key H(size_t j) { return Symbol('h', j); } +inline Key I(size_t j) { return Symbol('i', j); } +inline Key J(size_t j) { return Symbol('j', j); } +inline Key K(size_t j) { return Symbol('k', j); } +inline Key L(size_t j) { return Symbol('l', j); } +inline Key M(size_t j) { return Symbol('m', j); } +inline Key N(size_t j) { return Symbol('n', j); } +inline Key O(size_t j) { return Symbol('o', j); } +inline Key P(size_t j) { return Symbol('p', j); } +inline Key Q(size_t j) { return Symbol('q', j); } +inline Key R(size_t j) { return Symbol('r', j); } +inline Key S(size_t j) { return Symbol('s', j); } +inline Key T(size_t j) { return Symbol('t', j); } +inline Key U(size_t j) { return Symbol('u', j); } +inline Key V(size_t j) { return Symbol('v', j); } +inline Key W(size_t j) { return Symbol('w', j); } +inline Key X(size_t j) { return Symbol('x', j); } +inline Key Y(size_t j) { return Symbol('y', j); } +inline Key Z(size_t j) { return Symbol('z', j); } +} + } // namespace gtsam diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index dd5b29fbd..a31e100ef 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -22,9 +22,9 @@ * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. */ +#pragma once + #include -#include -#include #include #include // Only so Eclipse finds class definition diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 6a0d9b8d9..c3d3bd8df 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -106,6 +106,17 @@ namespace gtsam { } } + /* ************************************************************************* */ + const Value& Values::at(Key j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("retrieve", j); + return *item->second; + } + /* ************************************************************************* */ void Values::insert(Key j, const Value& val) { Key key = j; // Non-const duplicate to deal with non-const insert argument diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index b8807646c..bdd1739da 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -34,9 +34,7 @@ #include #include #include -#include -#include -#include +#include #include #include @@ -124,63 +122,125 @@ namespace gtsam { struct _KeyValuePair { const Key key; ///< The key ValueType& value; ///< The value - const Key& first; ///< For std::pair compatibility, the key - ValueType& second; ///< For std::pair compatibility, the value - _KeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value), first(key), second(value) {} + _KeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {} }; template struct _ConstKeyValuePair { const Key key; ///< The key const ValueType& value; ///< The value - const Key& first; ///< For std::pair compatibility, the key - const ValueType& second; ///< For std::pair compatibility, the value - _ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value), first(key), second(value) {} + _ConstKeyValuePair(Key _key, const ValueType& _value) : key(_key), value(_value) {} + _ConstKeyValuePair(const _KeyValuePair& rhs) : key(rhs.key), value(rhs.value) {} }; public: + template - class Filtered : public boost::transformed_range< - _KeyValuePair(*)(Values::KeyValuePair key_value), - const boost::filtered_range< - boost::function, - const boost::iterator_range > > { + class Filtered { public: + /** A key-value pair, with the value a specific derived Value type. */ typedef _KeyValuePair KeyValuePair; + typedef _ConstKeyValuePair ConstKeyValuePair; + + typedef + boost::transform_iterator< + KeyValuePair(*)(Values::KeyValuePair), + boost::filter_iterator< + boost::function, + Values::iterator> > + iterator; + + typedef iterator const_iterator; + + typedef + boost::transform_iterator< + ConstKeyValuePair(*)(Values::ConstKeyValuePair), + boost::filter_iterator< + boost::function, + Values::const_iterator> > + const_const_iterator; + + iterator begin() { return begin_; } + iterator end() { return end_; } + const_iterator begin() const { return begin_; } + const_iterator end() const { return end_; } + const_const_iterator beginConst() const { return constBegin_; } + const_const_iterator endConst() const { return constEnd_; } + + /** Returns the number of values in this view */ + size_t size() const { + size_t i = 0; + for (const_const_iterator it = beginConst(); it != endConst(); ++it) + ++i; + return i; + } private: - typedef boost::transformed_range< - KeyValuePair(*)(Values::KeyValuePair key_value), - const boost::filtered_range< - boost::function, - const boost::iterator_range > > Base; - - Filtered(const Base& base) : Base(base) {} + Filtered(const boost::function& filter, Values& values) : + begin_(boost::make_transform_iterator( + boost::make_filter_iterator( + filter, values.begin(), values.end()), + &castHelper)), + end_(boost::make_transform_iterator( + boost::make_filter_iterator( + filter, values.end(), values.end()), + &castHelper)), + constBegin_(boost::make_transform_iterator( + boost::make_filter_iterator( + filter, ((const Values&)values).begin(), ((const Values&)values).end()), + &castHelper)), + constEnd_(boost::make_transform_iterator( + boost::make_filter_iterator( + filter, ((const Values&)values).end(), ((const Values&)values).end()), + &castHelper)) {} friend class Values; + iterator begin_; + iterator end_; + const_const_iterator constBegin_; + const_const_iterator constEnd_; }; template - class ConstFiltered : public boost::transformed_range< - _ConstKeyValuePair(*)(Values::ConstKeyValuePair key_value), - const boost::filtered_range< - boost::function, - const boost::iterator_range > > { + class ConstFiltered { public: + /** A const key-value pair, with the value a specific derived Value type. */ typedef _ConstKeyValuePair KeyValuePair; + typedef typename Filtered::const_const_iterator iterator; + typedef typename Filtered::const_const_iterator const_iterator; + + /** Conversion from Filtered to ConstFiltered */ + ConstFiltered(const Filtered& rhs) : + begin_(rhs.beginConst()), + end_(rhs.endConst()) {} + + iterator begin() { return begin_; } + iterator end() { return end_; } + const_iterator begin() const { return begin_; } + const_iterator end() const { return end_; } + + /** Returns the number of values in this view */ + size_t size() const { + size_t i = 0; + for (const_iterator it = begin(); it != end(); ++it) + ++i; + return i; + } + private: - typedef boost::transformed_range< - KeyValuePair(*)(Values::ConstKeyValuePair key_value), - const boost::filtered_range< - boost::function, - const boost::iterator_range > > Base; - - ConstFiltered(const Base& base) : Base(base) {} - friend class Values; + const_iterator begin_; + const_iterator end_; + ConstFiltered(const boost::function& filter, const Values& values) { + // We remove the const from values to create a non-const Filtered + // view, then pull the const_iterators out of it. + const Filtered filtered(filter, const_cast(values)); + begin_ = filtered.beginConst(); + end_ = filtered.endConst(); + } }; /** Default constructor creates an empty Values class */ @@ -189,6 +249,24 @@ namespace gtsam { /** Copy constructor duplicates all keys and values */ Values(const Values& other); + /** Constructor from a Filtered view copies out all values */ + template + Values(const Filtered& view) { + BOOST_FOREACH(const typename Filtered::KeyValuePair& key_value, view) { + Key key = key_value.key; + insert(key, key_value.value); + } + } + + /** Constructor from Const Filtered view */ + template + Values(const ConstFiltered& view) { + BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { + Key key = key_value.key; + insert(key, key_value.value); + } + } + /// @name Testable /// @{ @@ -210,6 +288,13 @@ namespace gtsam { template const ValueType& at(Key j) const; + /** Retrieve a variable by key \c j. This version returns a reference + * to the base Value class, and needs to be casted before use. + * @param j Retrieve the value associated with this key + * @return A const reference to the stored value + */ + const Value& at(Key j) const; + #if 0 /** Retrieve a variable by key \c j. This non-templated version returns a * special ValueAutomaticCasting object that may be assigned to the proper @@ -358,12 +443,7 @@ namespace gtsam { template Filtered filter(const boost::function& filterFcn = (boost::lambda::_1, true)) { - return boost::adaptors::transform( - boost::adaptors::filter( - boost::make_iterator_range(begin(), end()), - boost::function( - boost::bind(&filterHelper, filterFcn, _1))), - &castHelper, KeyValuePair>); + return Filtered(boost::bind(&filterHelper, filterFcn, _1), *this); } /** @@ -405,12 +485,7 @@ namespace gtsam { template ConstFiltered filter(const boost::function& filterFcn = (boost::lambda::_1, true)) const { - return boost::adaptors::transform( - boost::adaptors::filter( - boost::make_iterator_range(begin(), end()), - boost::function( - boost::bind(&filterHelper, filterFcn, _1))), - &castHelper, ConstKeyValuePair>); + return ConstFiltered(boost::bind(&filterHelper, filterFcn, _1), *this); } private: diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/nonlinear/tests/testKey.cpp index 397bf9958..54a2e504a 100644 --- a/gtsam/nonlinear/tests/testKey.cpp +++ b/gtsam/nonlinear/tests/testKey.cpp @@ -44,17 +44,17 @@ TEST(Key, KeySymbolEncoding) { Key key = 0x6100000000000005; string str = "a5"; - LONGS_EQUAL(key, (Key)symbol); - assert_equal(str, DefaultKeyFormatter(symbol)); - assert_equal(symbol, Symbol(key)); + EXPECT_LONGS_EQUAL(key, (Key)symbol); + EXPECT(assert_equal(str, DefaultKeyFormatter(symbol))); + EXPECT(assert_equal(symbol, Symbol(key))); } else if(sizeof(Key) == 4) { Symbol symbol(0x61, 5); Key key = 0x61000005; string str = "a5"; - LONGS_EQUAL(key, (Key)symbol); - assert_equal(str, DefaultKeyFormatter(symbol)); - assert_equal(symbol, Symbol(key)); + EXPECT_LONGS_EQUAL(key, (Key)symbol); + EXPECT(assert_equal(str, DefaultKeyFormatter(symbol))); + EXPECT(assert_equal(symbol, Symbol(key))); } } diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp index 8c1804016..f57e3281a 100644 --- a/gtsam/nonlinear/tests/testOrdering.cpp +++ b/gtsam/nonlinear/tests/testOrdering.cpp @@ -18,6 +18,7 @@ #include #include +using namespace std; using namespace gtsam; /* ************************************************************************* */ @@ -51,6 +52,25 @@ TEST( testOrdering, simple_modifications ) { EXPECT(assert_equal(expectedFinal, ordering)); } +/* ************************************************************************* */ +TEST( testOrdering, invert ) { + // creates a map with the opposite mapping: Index->Key + Ordering ordering; + + // create an ordering + Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4); + ordering += x1, x2, x3, x4; + + Ordering::InvertedMap actual = ordering.invert(); + Ordering::InvertedMap expected; + expected.insert(make_pair(0, x1)); + expected.insert(make_pair(1, x2)); + expected.insert(make_pair(2, x3)); + expected.insert(make_pair(3, x4)); + + EXPECT(assert_container_equality(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index c37759552..6ccd3b083 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -274,6 +274,7 @@ TEST(Values, filter) { // Filter by key int i = 0; Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + EXPECT_LONGS_EQUAL(2, filtered.size()); BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { if(i == 0) { LONGS_EQUAL(2, key_value.key); @@ -288,23 +289,39 @@ TEST(Values, filter) { } ++ i; } - LONGS_EQUAL(2, i); + EXPECT_LONGS_EQUAL(2, i); + + // construct a values with the view + Values actualSubValues1(filtered); + Values expectedSubValues1; + expectedSubValues1.insert(2, pose2); + expectedSubValues1.insert(3, pose3); + EXPECT(assert_equal(expectedSubValues1, actualSubValues1)); // Filter by type i = 0; - BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter()) { + Values::ConstFiltered pose_filtered = values.filter(); + EXPECT_LONGS_EQUAL(2, pose_filtered.size()); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, pose_filtered) { if(i == 0) { - LONGS_EQUAL(1, key_value.key); + EXPECT_LONGS_EQUAL(1, key_value.key); EXPECT(assert_equal(pose1, key_value.value)); } else if(i == 1) { - LONGS_EQUAL(3, key_value.key); + EXPECT_LONGS_EQUAL(3, key_value.key); EXPECT(assert_equal(pose3, key_value.value)); } else { EXPECT(false); } ++ i; } - LONGS_EQUAL(2, i); + EXPECT_LONGS_EQUAL(2, i); + + // construct a values with the view + Values actualSubValues2(pose_filtered); + Values expectedSubValues2; + expectedSubValues2.insert(1, pose1); + expectedSubValues2.insert(3, pose3); + EXPECT(assert_equal(expectedSubValues2, actualSubValues2)); } /* ************************************************************************* */ diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index f4251273d..284d10a8c 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -63,10 +63,13 @@ namespace gtsam { * @param K shared pointer to the constant calibration */ GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model, - const Symbol poseKey, Key pointKey, const shared_ptrK& K) : + const Key poseKey, Key pointKey, const shared_ptrK& K) : Base(model, poseKey, pointKey), measured_(measured), K_(K) { } + /** Virtual destructor */ + virtual ~GenericProjectionFactor() {} + /** * print * @param s optional string naming the factor diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 1eefacc96..ae2deeae3 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -54,7 +54,7 @@ public: Base(model, poseKey, landmarkKey), measured_(measured), K_(K) { } - ~GenericStereoFactor() {} ///< destructor + virtual ~GenericStereoFactor() {} ///< Virtual destructor /** * print diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 3daf5f4ab..b32bf9a9c 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -126,6 +126,13 @@ TEST( planarSLAM, BearingRangeFactor_equals ) EXPECT(assert_inequal(factor1, factor2, 1e-5)); } +/* ************************************************************************* */ +TEST( planarSLAM, BearingRangeFactor_poses ) +{ + typedef BearingRangeFactor PoseBearingRange; + PoseBearingRange actual(PoseKey(2), PoseKey(3), Rot2::fromDegrees(60.0), 12.3, sigma2); +} + /* ************************************************************************* */ TEST( planarSLAM, PoseConstraint_equals ) { diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 8a28c8dad..02b4d455e 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -25,7 +25,7 @@ if (GTSAM_BUILD_TESTS) list(REMOVE_ITEM tests_srcs ${tests_exclude}) foreach(test_src ${tests_srcs}) get_filename_component(test_base ${test_src} NAME_WE) - set( test_bin tests.${test_base} ) + set( test_bin ${test_base} ) message(STATUS "Adding Test ${test_bin}") add_executable(${test_bin} ${test_src}) add_dependencies(check.tests ${test_bin}) @@ -43,7 +43,7 @@ if (GTSAM_BUILD_TIMING) list(REMOVE_ITEM timing_srcs ${tests_exclude}) foreach(time_src ${timing_srcs}) get_filename_component(time_base ${time_src} NAME_WE) - set( time_bin tests.${time_base} ) + set( time_bin ${time_base} ) message(STATUS "Adding Timing Benchmark ${time_bin}") add_executable(${time_bin} ${time_src}) add_dependencies(timing.tests ${time_bin}) diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 27070b801..b16ac0cdc 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -102,7 +103,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { VectorValues expected = gradientValues; scal(step, expected); // Compute the steepest descent point with the dogleg function - VectorValues actual = DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn); + VectorValues actual = optimizeGradientSearch(gbn); // Check that points agree EXPECT(assert_equal(expected, actual, 1e-5)); @@ -290,7 +291,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { expectedFromBN[4] = Vector_(2, 0.300134, 0.423233); // Compute the steepest descent point with the dogleg function - VectorValues actual = DoglegOptimizerImpl::ComputeSteepestDescentPoint(bt); + VectorValues actual = optimizeGradientSearch(bt); // Check that points agree EXPECT(assert_equal(expected, actual, 1e-5)); @@ -324,7 +325,7 @@ TEST(DoglegOptimizer, ComputeBlend) { 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); // Compute steepest descent point - VectorValues xu = DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn); + VectorValues xu = optimizeGradientSearch(gbn); // Compute Newton's method point VectorValues xn = optimize(gbn); @@ -362,18 +363,18 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) { // Compute dogleg point for different deltas double Delta1 = 0.5; // Less than steepest descent - VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn)); + VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn)); DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5); double Delta2 = 1.5; // Between steepest descent and Newton's method - VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn)); - VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn)); + VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); + VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5); EXPECT(assert_equal(expected2, actual2)); double Delta3 = 5.0; // Larger than Newton's method point VectorValues expected3 = optimize(gbn); - VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn)); + VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn)); EXPECT(assert_equal(expected3, actual3)); } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 927499050..d9e5643a4 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -362,8 +362,8 @@ public: TEST( ExtendedKalmanFilter, nonlinear ) { // Create the set of expected output TestValues (generated using Matlab Kalman Filter) - Point2 expected_predict[11]; - Point2 expected_update[11]; + Point2 expected_predict[10]; + Point2 expected_update[10]; expected_predict[0] = Point2(0.81,0.99); expected_update[0] = Point2(0.824926197027,0.29509808); expected_predict[1] = Point2(0.680503230541,0.24343413); @@ -409,11 +409,11 @@ TEST( ExtendedKalmanFilter, nonlinear ) { Point2 x_predict, x_update; for(unsigned int i = 0; i < 10; ++i){ // Create motion factor - NonlinearMotionModel motionFactor(Symbol('x',i-1), Symbol('x',i)); + NonlinearMotionModel motionFactor(Symbol('x',i), Symbol('x',i+1)); x_predict = ekf.predict(motionFactor); // Create a measurement factor - NonlinearMeasurementModel measurementFactor(Symbol('x',i), Vector_(1, z[i])); + NonlinearMeasurementModel measurementFactor(Symbol('x',i+1), Vector_(1, z[i])); x_update = ekf.update(measurementFactor); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp index f0cec7530..8444dacc3 100644 --- a/tests/testGaussianBayesNet.cpp +++ b/tests/testGaussianBayesNet.cpp @@ -119,64 +119,25 @@ TEST( GaussianBayesNet, optimize2 ) } /* ************************************************************************* */ -TEST( GaussianBayesNet, backSubstitute ) +TEST( GaussianBayesNet, optimize3 ) { // y=R*x, x=inv(R)*y - // 2 = 1 1 -1 - // 3 1 3 + // 9 = 1 1 4 + // 5 1 5 // NOTE: we are supplying a new RHS here GaussianBayesNet cbn = createSmallGaussianBayesNet(); - VectorValues y(vector(2,1)), x(vector(2,1)); - y[_x_] = Vector_(1,2.); - y[_y_] = Vector_(1,3.); - x[_x_] = Vector_(1,-1.); - x[_y_] = Vector_(1, 3.); + VectorValues expected(vector(2,1)), x(vector(2,1)); + expected[_x_] = Vector_(1, 4.); + expected[_y_] = Vector_(1, 5.); // test functional version - VectorValues actual = backSubstitute(cbn,y); - EXPECT(assert_equal(x,actual)); + VectorValues actual = optimize(cbn); + EXPECT(assert_equal(expected,actual)); // test imperative version - backSubstituteInPlace(cbn,y); - EXPECT(assert_equal(x,y)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNet, rhs ) -{ - // y=R*x, x=inv(R)*y - // 2 = 1 1 -1 - // 3 1 3 - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - VectorValues expected = gtsam::optimize(cbn); - VectorValues d = rhs(cbn); - VectorValues actual = backSubstitute(cbn, d); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNet, rhs_with_sigmas ) -{ - Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); - Matrix R22 = Matrix_(1, 1, 1.0); - Vector d1(1), d2(1); - d1(0) = 9; - d2(0) = 5; - Vector tau(1); - tau(0) = 0.25; - - // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, - _y_, S12, tau)), Py(new GaussianConditional(_y_, d2, R22, tau)); - GaussianBayesNet cbn; - cbn.push_back(Px_y); - cbn.push_back(Py); - - VectorValues expected = gtsam::optimize(cbn); - VectorValues d = rhs(cbn); - VectorValues actual = backSubstitute(cbn, d); - EXPECT(assert_equal(expected, actual)); + optimizeInPlace(cbn,x); + EXPECT(assert_equal(expected,x)); } /* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraphB.cpp similarity index 85% rename from tests/testGaussianFactorGraph.cpp rename to tests/testGaussianFactorGraphB.cpp index 671d09cc7..8d7d96ea4 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testGaussianFactorGraph.cpp + * @file testGaussianFactorGraphB.cpp * @brief Unit tests for Linear Factor Graph * @author Christian Potthast **/ @@ -192,96 +192,116 @@ TEST( GaussianFactorGraph, equals ) { // EXPECT(assert_equal(expected,*actual)); //} -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_x1 ) -//{ -// Ordering ordering; ordering += kx(1),kl(1),kx(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); -// -// // create expected Conditional Gaussian -// Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; -// Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); -// GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} -// -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_x2 ) -//{ -// Ordering ordering; ordering += kx(2),kl(1),kx(1); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); -// -// // create expected Conditional Gaussian -// double sig = 0.0894427; -// Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; -// Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); -// GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} -// -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_l1 ) -//{ -// Ordering ordering; ordering += kl(1),kx(1),kx(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); -// -// // create expected Conditional Gaussian -// double sig = sqrt(2)/10.; -// Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; -// Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); -// GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x1 ) +{ + Ordering ordering; ordering += kx(1),kl(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_x1_fast ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(1), false); -// -// // create expected Conditional Gaussian -// Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; -// Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); -// GaussianConditional expected(kx(1),15*d,R11,kl(1),S12,kx(2),S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} -// -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_x2_fast ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(2), false); -// -// // create expected Conditional Gaussian -// double sig = 0.0894427; -// Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; -// Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); -// GaussianConditional expected(kx(2),d,R11,kl(1),S12,kx(1),S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} -// -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_l1_fast ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne(kl(1), false); -// -// // create expected Conditional Gaussian -// double sig = sqrt(2)/10.; -// Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; -// Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); -// GaussianConditional expected(kl(1),d,R11,kx(1),S12,kx(2),S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} + GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR); + + // create expected Conditional Gaussian + Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); + GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); + + EXPECT(assert_equal(expected,*result.first,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x2 ) +{ + Ordering ordering; ordering += kx(2),kl(1),kx(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; + + // create expected Conditional Gaussian + double sig = 0.0894427; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); + GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma); + + EXPECT(assert_equal(expected,*actual,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_l1 ) +{ + Ordering ordering; ordering += kl(1),kx(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; + + // create expected Conditional Gaussian + double sig = sqrt(2)/10.; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); + GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); + + EXPECT(assert_equal(expected,*actual,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x1_fast ) +{ + Ordering ordering; ordering += kx(1),kl(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[kx(1)], EliminateQR); + GaussianConditional::shared_ptr conditional = result.first; + GaussianFactorGraph remaining = result.second; + + // create expected Conditional Gaussian + Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); + GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); + + // Create expected remaining new factor + JacobianFactor expectedFactor(1, Matrix_(4,2, + 4.714045207910318, 0., + 0., 4.714045207910318, + 0., 0., + 0., 0.), + 2, Matrix_(4,2, + -2.357022603955159, 0., + 0., -2.357022603955159, + 7.071067811865475, 0., + 0., 7.071067811865475), + Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), sharedUnit(4)); + + EXPECT(assert_equal(expected,*conditional,tol)); + EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x2_fast ) +{ + Ordering ordering; ordering += kx(1),kl(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kx(2)], EliminateQR).first; + + // create expected Conditional Gaussian + double sig = 0.0894427; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); + GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kx(1)],S13,ordering[kl(1)],S12,sigma); + + EXPECT(assert_equal(expected,*actual,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_l1_fast ) +{ + Ordering ordering; ordering += kx(1),kl(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(1)], EliminateQR).first; + + // create expected Conditional Gaussian + double sig = sqrt(2)/10.; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); + GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); + + EXPECT(assert_equal(expected,*actual,tol)); +} /* ************************************************************************* */ TEST( GaussianFactorGraph, eliminateAll ) @@ -439,7 +459,7 @@ TEST( GaussianFactorGraph, getOrdering) { Ordering original; original += kl(1),kx(1),kx(2); FactorGraph symbolic(createGaussianFactorGraph(original)); - Permutation perm(*Inference::PermutationCOLAMD(VariableIndex(symbolic))); + Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); Ordering expected; expected += kl(1),kx(2),kx(1); EXPECT(assert_equal(expected,actual)); diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index b02d68583..1df8cdff3 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -85,7 +85,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) // Ordering ord; ord += kx(4),kx(3),kx(2),kx(1); // GaussianFactorGraph factors1; // for (int i=0;i<7;i++) factors1.push_back(smoother[i]); -// GaussianISAM actual(*Inference::Eliminate(factors1)); +// GaussianISAM actual(*inference::Eliminate(factors1)); // // // run iSAM with remaining factors // GaussianFactorGraph factors2; @@ -298,7 +298,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) // varIndex.permute(toFront); // BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { // factor->permuteWithInverse(toFrontInverse); } -// GaussianBayesNet actual = *Inference::EliminateUntil(marginal, C3->keys().size(), varIndex); +// GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex); // actual.permuteWithInverse(toFront); // EXPECT(assert_equal(expected,actual,tol)); //} diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f41cff097..21f2e62e9 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -16,6 +16,7 @@ using namespace boost::assign; #include #include #include +#include #include #include #include @@ -47,6 +48,8 @@ TEST(ISAM2, AddVariables) { Permuted delta(permutation, deltaUnpermuted); + vector replacedKeys(2, false); + Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); GaussianISAM2<>::Nodes nodes(2); @@ -75,17 +78,20 @@ TEST(ISAM2, AddVariables) { Permuted deltaExpected(permutationExpected, deltaUnpermutedExpected); + vector replacedKeysExpected(3, false); + Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); GaussianISAM2<>::Nodes nodesExpected( 3, GaussianISAM2<>::sharedClique()); // Expand initial state - GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes); + GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation())); + EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); } @@ -162,15 +168,13 @@ TEST(ISAM2, optimize2) { // Expected vector VectorValues expected(1, 3); - conditional->rhs(expected); conditional->solveInPlace(expected); // Clique GaussianISAM2<>::sharedClique clique( GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); VectorValues actual(theta.dims(ordering)); - conditional->rhs(actual); - optimize2(clique, actual); + internal::optimizeInPlace(clique, actual); // expected.print("expected: "); // actual.print("actual: "); diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTreeB.cpp similarity index 99% rename from tests/testGaussianJunctionTree.cpp rename to tests/testGaussianJunctionTreeB.cpp index b01aa2ba4..c6b935c21 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testGaussianJunctionTree.cpp + * @file testGaussianJunctionTreeB.cpp * @date Jul 8, 2010 * @author nikai */ diff --git a/tests/testInference.cpp b/tests/testInferenceB.cpp similarity index 96% rename from tests/testInference.cpp rename to tests/testInferenceB.cpp index 0bc6c3782..6ad070698 100644 --- a/tests/testInference.cpp +++ b/tests/testInferenceB.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testInference.cpp + * @file testInferenceB.cpp * @brief Unit tests for functionality declared in inference.h * @author Frank Dellaert */ @@ -31,7 +31,7 @@ using namespace gtsam; /* ************************************************************************* */ /* ************************************************************************* */ -TEST( Inference, marginals ) +TEST( inference, marginals ) { using namespace example; // create and marginalize a small Bayes net on "x" @@ -46,7 +46,7 @@ TEST( Inference, marginals ) } /* ************************************************************************* */ -TEST( Inference, marginals2) +TEST( inference, marginals2) { planarSLAM::Graph fg; SharedDiagonal poseModel(sharedSigma(3, 0.1)); diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNetB.cpp similarity index 80% rename from tests/testSymbolicBayesNet.cpp rename to tests/testSymbolicBayesNetB.cpp index 200530e6d..e25cf33ca 100644 --- a/tests/testSymbolicBayesNet.cpp +++ b/tests/testSymbolicBayesNetB.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testSymbolicBayesNet.cpp + * @file testSymbolicBayesNetB.cpp * @brief Unit tests for a symbolic Bayes chain * @author Frank Dellaert */ @@ -34,11 +34,6 @@ using namespace example; Key kx(size_t i) { return Symbol('x',i); } Key kl(size_t i) { return Symbol('l',i); } -//Symbol _B_('B', 0), _L_('L', 0); -//IndexConditional::shared_ptr -// B(new IndexConditional(_B_)), -// L(new IndexConditional(_L_, _B_)); - /* ************************************************************************* */ TEST( SymbolicBayesNet, constructor ) { @@ -64,6 +59,18 @@ TEST( SymbolicBayesNet, constructor ) CHECK(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST( SymbolicBayesNet, FromGaussian) { + SymbolicBayesNet expected; + expected.push_back(IndexConditional::shared_ptr(new IndexConditional(0, 1))); + expected.push_back(IndexConditional::shared_ptr(new IndexConditional(1))); + + GaussianBayesNet gbn = createSmallGaussianBayesNet(); + SymbolicBayesNet actual(gbn); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraphB.cpp similarity index 99% rename from tests/testSymbolicFactorGraph.cpp rename to tests/testSymbolicFactorGraphB.cpp index f8fa777d1..9b195e0c1 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraphB.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testSymbolicFactorGraph.cpp + * @file testSymbolicFactorGraphB.cpp * @brief Unit tests for a symbolic Factor Graph * @author Frank Dellaert */ diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index de89ca5ee..21f39fdb5 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -8,7 +8,6 @@ add_executable(wrap wrap.cpp) target_link_libraries(wrap wrap_lib) # Install wrap binary -option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON) if (GTSAM_INSTALL_WRAP) install(TARGETS wrap DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) endif(GTSAM_INSTALL_WRAP) @@ -34,61 +33,14 @@ set(mexFlags "-I${Boost_INCLUDE_DIR} -I${CMAKE_INSTALL_PREFIX}/include -I${CMAKE set(toolbox_path ${CMAKE_BINARY_DIR}/wrap/gtsam) set(moduleName gtsam) -## Determine the mex extension -# Apple Macintosh (64-bit) mexmaci64 -# Linux (32-bit) mexglx -# Linux (64-bit) mexa64 -# Microsoft Windows (32-bit) mexw32 -# Windows (64-bit) mexw64 +include(GtsamMatlabWrap) +find_mexextension() -# only support 64-bit apple -if(CMAKE_HOST_APPLE) - set(GTSAM_MEX_BIN_EXTENSION_default mexmaci64) -endif(CMAKE_HOST_APPLE) - -if(NOT CMAKE_HOST_APPLE) - # check 64 bit - if( ${CMAKE_SIZEOF_VOID_P} EQUAL 4 ) - set( HAVE_64_BIT 0 ) - endif( ${CMAKE_SIZEOF_VOID_P} EQUAL 4 ) - - if( ${CMAKE_SIZEOF_VOID_P} EQUAL 8 ) - set( HAVE_64_BIT 1 ) - endif( ${CMAKE_SIZEOF_VOID_P} EQUAL 8 ) - - # Check for linux machines - if (CMAKE_HOST_UNIX) - if (HAVE_64_BIT) - set(GTSAM_MEX_BIN_EXTENSION_default mexa64) - else (HAVE_64_BIT) - set(GTSAM_MEX_BIN_EXTENSION_default mexglx) - endif (HAVE_64_BIT) - endif(CMAKE_HOST_UNIX) - - # Check for windows machines - if (CMAKE_HOST_WIN32) - if (HAVE_64_BIT) - set(GTSAM_MEX_BIN_EXTENSION_default mexw64) - else (HAVE_64_BIT) - set(GTSAM_MEX_BIN_EXTENSION_default mexw32) - endif (HAVE_64_BIT) - endif(CMAKE_HOST_WIN32) -endif(NOT CMAKE_HOST_APPLE) - -# Allow for setting mex extension manually -set(GTSAM_MEX_BIN_EXTENSION ${GTSAM_MEX_BIN_EXTENSION_default} CACHE DOCSTRING "Extension for matlab mex files") -message(STATUS "Detected Matlab mex extension: ${GTSAM_MEX_BIN_EXTENSION_default}") -message(STATUS "Current Matlab mex extension: ${GTSAM_MEX_BIN_EXTENSION}") - -# Actual build commands - separated by OS +# Code generation command add_custom_target(wrap_gtsam ALL COMMAND ./wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_SOURCE_DIR} ${moduleName} ${toolbox_path} "${mexFlags}" DEPENDS wrap) -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON) -option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON) -option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON) - set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox") if (GTSAM_INSTALL_MATLAB_TOOLBOX)