diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c4f8e230..337fd19a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,8 +52,7 @@ option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) endif() -option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) -option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" OFF) +option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead of shared" OFF) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) @@ -76,15 +75,14 @@ if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP) message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP") endif() +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.") +endif() + # Flags for choosing default packaging tools set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") -# Sanity check building of libraries -if (NOT GTSAM_BUILD_SHARED_LIBRARY AND NOT GTSAM_BUILD_STATIC_LIBRARY) - message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!") -endif() - # Flags to determine whether tests and examples are build during 'make install' # Note that these remove the targets from the 'all' option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON) @@ -171,23 +169,27 @@ find_package(GooglePerfTools) ############################################################################### # Find MKL -find_package(MKL) +if(GTSAM_USE_EIGEN_MKL) + find_package(MKL) -if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) - set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h - set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL - include_directories(${MKL_INCLUDE_DIR}) - list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) + if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) + set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h + set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL + include_directories(${MKL_INCLUDE_DIR}) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) + endif() endif() ############################################################################### -# Find OpenMP -find_package(OpenMP) +# Find OpenMP (if we're also using MKL) +if(GTSAM_USE_EIGEN_MKL AND GTSAM_USE_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) + find_package(OpenMP) -if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) - set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) + set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + endif() endif() @@ -286,24 +288,11 @@ endif() if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) -endif() +endif() ############################################################################### # Add components -# Set default library - static or shared, before adding subdirectories -if(GTSAM_BUILD_SHARED_LIBRARY) - set(gtsam-default gtsam-shared) - if(GTSAM_BUILD_UNSTABLE) - set(gtsam_unstable-default gtsam_unstable-shared) - endif() -else() - set(gtsam-default gtsam-static) - if(GTSAM_BUILD_UNSTABLE) - set(gtsam_unstable-default gtsam_unstable-static) - endif() -endif() - # Build CppUnitLite add_subdirectory(CppUnitLite) @@ -351,7 +340,7 @@ endif() set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") -set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README") +set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") 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}) @@ -381,8 +370,7 @@ print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests if (DOXYGEN_FOUND) print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") endif() -print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") -print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ") +print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM library instead of shared") print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ") if(GTSAM_UNSTABLE_AVAILABLE) print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") @@ -390,9 +378,11 @@ endif() print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "No tests in all or install ") print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "No examples in all or install ") 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}}") +if(NOT MSVC AND NOT XCODE_VERSION) + 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}}") +endif() if(GTSAM_USE_TBB) message(STATUS " Use Intel TBB : Yes") elseif(TBB_FOUND) diff --git a/README b/INSTALL similarity index 74% rename from README rename to INSTALL index 1aba92b31..75277e815 100644 --- a/README +++ b/INSTALL @@ -1,6 +1,3 @@ -README - Georgia Tech Smoothing and Mapping library ---------------------------------------------------- - Quickstart In the root library folder execute: @@ -11,42 +8,6 @@ $] cmake .. $] make check (optional, runs unit tests) $] make install ---------------------------------------------------- -What is GTSAM? - - GTSAM is a library of C++ classes that implement smoothing and - mapping (SAM) in robotics and vision, using factor graphs and Bayes - networks as the underlying computing paradigm rather than sparse - matrices. - -GTSAM is not (yet) open source: See COPYING & LICENSE -Please see USAGE for an example on how to use GTSAM. - -The library is organized according to the following directory structure: - - 3rdparty local copies of third party libraries - Eigen3 and CCOLAMD - base provides some base Math and data structures, as well as test-related utilities - geometry points, poses, tensors, etc - inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees - linear inference specialized to Gaussian linear case, GaussianFactorGraph etc... - nonlinear non-linear factor graphs and non-linear optimization - slam SLAM and visual SLAM application code - -This library contains unchanged copies of two third party libraries, with documentation -of licensing as follows: - - CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library - - http://www.cise.ufl.edu/research/sparse - - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt - - Eigen 3.2: General C++ matrix and linear algebra library - - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README (some code that is 3rd-party to - Eigen is BSD and LGPL) - - -There are two supporting libraries: - - CppUnitLite unit test library customized for use with gtsam - wrap code generation utility for the Matlab interface to gtsam - Important Installation Notes ---------------------------- @@ -64,15 +25,17 @@ Optional dependent libraries: may be downloaded from https://www.threadingbuildingblocks.org/ Tested compilers - - GCC 4.2-4.7 - - Clang 2.9-3.2 - - OSX GCC 4.2 - - MSVC 2010, 2012 - + +- GCC 4.2-4.7 +- OSX Clang 2.9-5.0 +- OSX GCC 4.2 +- MSVC 2010, 2012 + Tested systems: - - Ubuntu 11.04, 11.10, 12.04, 12.10, 13.04 - - MacOS 10.6, 10.7 - - Windows 7 + +- Ubuntu 11.04 - 13.10 +- MacOS 10.6 - 10.9 +- Windows 7, 8 2) GTSAM makes extensive use of debug assertions, and we highly recommend you work diff --git a/LICENSE b/LICENSE index 81c000246..e7424bbc2 100644 --- a/LICENSE +++ b/LICENSE @@ -1,29 +1,18 @@ -Copyright (c) 2010, Georgia Tech Research Corporation -Atlanta, Georgia 30332-0415 -All Rights Reserved +GTSAM is released under the simplified BSD license, reproduced in the file +LICENSE.BSD in this directory. -See also README for licensing of 3rd-party code included in GTSAM. +GTSAM contains two third party libraries, with documentation of licensing and +modifications as follows: -Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of the copyright holders nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree +ordering library + - Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig + - http://www.cise.ufl.edu/research/sparse + - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt +- Eigen 3.2: General C++ matrix and linear algebra library + - Modified with 3 patches that have been contributed back to the Eigen team: + - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) + - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) + - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization) + - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README + - Some code that is 3rd-party to Eigen is BSD and LGPL \ No newline at end of file diff --git a/LICENSE.BSD b/LICENSE.BSD index 81c000246..406b266b7 100644 --- a/LICENSE.BSD +++ b/LICENSE.BSD @@ -2,28 +2,12 @@ Copyright (c) 2010, Georgia Tech Research Corporation Atlanta, Georgia 30332-0415 All Rights Reserved -See also README for licensing of 3rd-party code included in GTSAM. +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. - * Neither the name of the copyright holders nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/README.md b/README.md index 77c592e43..460f51bf3 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,20 @@ README - Georgia Tech Smoothing and Mapping library =================================================== +What is GTSAM? +-------------- + +GTSAM is a library of C++ classes that implement smoothing and +mapping (SAM) in robotics and vision, using factor graphs and Bayes +networks as the underlying computing paradigm rather than sparse +matrices. + +On top of the C++ library, GTSAM includes a MATLAB interface (enable +GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface +is under development. + Quickstart +---------- In the root library folder execute: @@ -24,52 +37,11 @@ Optional prerequisites - used automatically if findable by CMake: - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) - [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) -Tested compilers +Additional Information +---------------------- -- GCC 4.2-4.7 -- OSX Clang 2.9-5.0 -- OSX GCC 4.2 -- MSVC 2010, 2012 +See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions. -Tested systems: +GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files. -- Ubuntu 11.04 - 13.10 -- MacOS 10.6 - 10.9 -- Windows 7, 8 - -See the `INSTALL` file for more detailed installation instructions. - -What is GTSAM? -============== - -GTSAM is a library of C++ classes that implement smoothing and -mapping (SAM) in robotics and vision, using factor graphs and Bayes -networks as the underlying computing paradigm rather than sparse -matrices. - -On top of the C++ library, GTSAM includes a MATLAB interface (enable -GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface -is under development. - -GTSAM is open source under the BSD license, see the `LICENSE.BSD` file. - -Please see the `examples/` directory and the `USAGE` file for examples on how to use GTSAM. - -The library is organized according to the following directory structure: - - 3rdparty local copies of third party libraries - Eigen3 and CCOLAMD - base provides some base Math and data structures, as well as test-related utilities - geometry points, poses, tensors, etc - inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees - linear inference specialized to Gaussian linear case, GaussianFactorGraph etc... - nonlinear non-linear factor graphs and non-linear optimization - slam SLAM and visual SLAM application code - -This library contains unchanged copies of two third party libraries, with documentation -of licensing as follows: - -- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library - - http://www.cise.ufl.edu/research/sparse - - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt -- Eigen 3.2: General C++ matrix and linear algebra library - - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README (some code that is 3rd-party to Eigen is BSD and LGPL) +Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM. diff --git a/USAGE b/USAGE index e5f4dc581..a41b71045 100644 --- a/USAGE +++ b/USAGE @@ -47,6 +47,17 @@ Factors: SLAM example, is a measurement such as a visual reading on a landmark or odometry. +The library is organized according to the following directory structure: + + 3rdparty local copies of third party libraries - Eigen3 and CCOLAMD + base provides some base Math and data structures, as well as test-related utilities + geometry points, poses, tensors, etc + inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees + linear inference specialized to Gaussian linear case, GaussianFactorGraph etc... + nonlinear non-linear factor graphs and non-linear optimization + slam SLAM and visual SLAM application code + + VSLAM Example --------------------------------------------------- diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 1fc259724..f00f3b3b1 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -10,11 +10,7 @@ if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSI endif() # Add option for using build type postfixes to allow installing multiple build modes -if(MSVC OR XCODE_VERSION) - option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) -else() - option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" OFF) -endif() +option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) # Add debugging flags but only on the first pass if(NOT FIRST_PASS_DONE) diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 25c2e25dd..f1b7c5e54 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -155,7 +155,7 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l endif() # Add .run target - if(NOT MSVC) + if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN}) endif() diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index eae90e298..d87070330 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -28,8 +28,8 @@ foreach(example_src ${example_srcs} ) set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON) endif() - target_link_libraries(${example_bin} ${gtsam-default} ${Boost_PROGRAM_OPTIONS_LIBRARY}) - if(NOT MSVC) + target_link_libraries(${example_bin} gtsam ${Boost_PROGRAM_OPTIONS_LIBRARY}) + if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) endif() diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b739f91c3..946ec5bc6 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -95,42 +95,40 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") # build shared and static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM - static") - add_library(gtsam-static STATIC ${gtsam_srcs}) - target_link_libraries(gtsam-static ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) - set_target_properties(gtsam-static PROPERTIES + add_library(gtsam STATIC ${gtsam_srcs}) + target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library - set_target_properties(gtsam-static PROPERTIES + set_target_properties(gtsam PROPERTIES PREFIX "lib" COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC) endif() - install(TARGETS gtsam-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib) - list(APPEND GTSAM_EXPORTED_TARGETS gtsam-static) + install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib) + list(APPEND GTSAM_EXPORTED_TARGETS gtsam) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -endif () - -if (GTSAM_BUILD_SHARED_LIBRARY) +else() message(STATUS "Building GTSAM - shared") - add_library(gtsam-shared SHARED ${gtsam_srcs}) - target_link_libraries(gtsam-shared ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) - set_target_properties(gtsam-shared PROPERTIES + add_library(gtsam SHARED ${gtsam_srcs}) + target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) if(WIN32) - set_target_properties(gtsam-shared PROPERTIES + set_target_properties(gtsam PROPERTIES PREFIX "" DEFINE_SYMBOL GTSAM_EXPORTS RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") endif() - install(TARGETS gtsam-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) - list(APPEND GTSAM_EXPORTED_TARGETS gtsam-shared) + install(TARGETS gtsam EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) + list(APPEND GTSAM_EXPORTED_TARGETS gtsam) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -endif(GTSAM_BUILD_SHARED_LIBRARY) +endif() # Set dataset paths set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" @@ -153,10 +151,10 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Generate, build and install toolbox set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") - if("${gtsam-default}" STREQUAL "gtsam-static") + if(GTSAM_BUILD_STATIC_LIBRARY) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) endif() # Wrap - wrap_and_install_library(../gtsam.h "${gtsam-default};${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}") + wrap_and_install_library(../gtsam.h "gtsam;${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}") endif () diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index a039a7481..94e2473ed 100644 --- a/gtsam/base/CMakeLists.txt +++ b/gtsam/base/CMakeLists.txt @@ -13,11 +13,11 @@ set(base_excluded_files # Build tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(base "${gtsam-default}" "${gtsam-default}" "${base_excluded_files}") + gtsam_add_subdir_tests(base "gtsam" "gtsam" "${base_excluded_files}") endif(GTSAM_BUILD_TESTS) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(base "${gtsam-default}" "${gtsam-default}" "${base_excluded_files}") + gtsam_add_subdir_timing(base "gtsam" "gtsam" "${base_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index a41182f47..89544cfd6 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -135,12 +135,12 @@ void deserializeBinary(const std::string& serialized, T& output, const std::stri } template -bool deserializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { +bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; boost::archive::binary_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input);; + out_archive << boost::serialization::make_nvp(name.c_str(), input); out_archive_stream.close(); return true; } diff --git a/gtsam/discrete/CMakeLists.txt b/gtsam/discrete/CMakeLists.txt index 4535868dd..3f1a57e0b 100644 --- a/gtsam/discrete/CMakeLists.txt +++ b/gtsam/discrete/CMakeLists.txt @@ -9,7 +9,7 @@ set (discrete_excluded_tests "") # Add all tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(discrete "${gtsam-default}" "${gtsam-default}" "${discrete_excluded_tests}") + gtsam_add_subdir_tests(discrete "gtsam" "gtsam" "${discrete_excluded_tests}") endif() # Build timing scripts diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt index b659d8314..96b84193b 100644 --- a/gtsam/geometry/CMakeLists.txt +++ b/gtsam/geometry/CMakeLists.txt @@ -10,11 +10,11 @@ set(geometry_excluded_files # Build tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(geometry "${gtsam-default}" "${gtsam-default}" "${geometry_excluded_files}") + gtsam_add_subdir_tests(geometry "gtsam" "gtsam" "${geometry_excluded_files}") endif(GTSAM_BUILD_TESTS) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(geometry "${gtsam-default}" "${gtsam-default}" "${geometry_excluded_files}") + gtsam_add_subdir_timing(geometry "gtsam" "gtsam" "${geometry_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index ef82e9369..72124d25d 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -73,7 +73,7 @@ Point3 Rot3::operator*(const Point3& p) const { /* ************************************************************************* */ Sphere2 Rot3::rotate(const Sphere2& p, boost::optional HR, boost::optional Hp) const { - Sphere2 q = rotate(p.point3(Hp)); + Sphere2 q = Sphere2(rotate(p.point3(Hp))); if (Hp) (*Hp) = q.basis().transpose() * matrix() * (*Hp); if (HR) @@ -84,7 +84,7 @@ Sphere2 Rot3::rotate(const Sphere2& p, /* ************************************************************************* */ Sphere2 Rot3::unrotate(const Sphere2& p, boost::optional HR, boost::optional Hp) const { - Sphere2 q = unrotate(p.point3(Hp)); + Sphere2 q = Sphere2(unrotate(p.point3(Hp))); if (Hp) (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); if (HR) diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index ac8124139..d4047f421 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -60,7 +60,7 @@ public: } /// Construct from point - Sphere2(const Point3& p) : + explicit Sphere2(const Point3& p) : p_(p / p.norm()) { } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 865f27f81..8c9eba4a8 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -22,17 +22,17 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) // Create two cameras and corresponding essential matrix E Rot3 c1Rc2 = Rot3::yaw(M_PI_2); Point3 c1Tc2(0.1, 0, 0); -EssentialMatrix trueE(c1Rc2, c1Tc2); +EssentialMatrix trueE(c1Rc2, Sphere2(c1Tc2)); //************************************************************************* TEST (EssentialMatrix, equality) { - EssentialMatrix actual(c1Rc2, c1Tc2), expected(c1Rc2, c1Tc2); + EssentialMatrix actual(c1Rc2, Sphere2(c1Tc2)), expected(c1Rc2, Sphere2(c1Tc2)); EXPECT(assert_equal(expected, actual)); } //************************************************************************* TEST (EssentialMatrix, retract1) { - EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), c1Tc2); + EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), Sphere2(c1Tc2)); EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0)); EXPECT(assert_equal(expected, actual)); } @@ -54,7 +54,7 @@ TEST (EssentialMatrix, transform_to) { Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4) * Rot3::roll(M_PI / 6.0); Point3 aTb2(19.2, 3.7, 5.9); - EssentialMatrix E(aRb2, aTb2); + EssentialMatrix E(aRb2, Sphere2(aTb2)); //EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0))); static Point3 P(0.2, 0.7, -2); Matrix actH1, actH2; @@ -79,7 +79,7 @@ TEST (EssentialMatrix, rotate) { // Let's compute the ground truth E in body frame: Rot3 b1Rb2 = bRc * c1Rc2 * cRb; Point3 b1Tb2 = bRc * c1Tc2; - EssentialMatrix bodyE(b1Rb2, b1Tb2); + EssentialMatrix bodyE(b1Rb2, Sphere2(b1Tb2)); EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8)); EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8)); @@ -111,7 +111,7 @@ TEST (EssentialMatrix, FromPose3_b) { Matrix actualH; Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); - EssentialMatrix trueE(c1Rc2, c1Tc2); + EssentialMatrix trueE(c1Rc2, Sphere2(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( @@ -121,7 +121,7 @@ TEST (EssentialMatrix, FromPose3_b) { //************************************************************************* TEST (EssentialMatrix, streaming) { - EssentialMatrix expected(c1Rc2, c1Tc2), actual; + EssentialMatrix expected(c1Rc2, Sphere2(c1Tc2)), actual; stringstream ss; ss << expected; ss >> actual; diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt index 8e63eecf8..d5c994b00 100644 --- a/gtsam/inference/CMakeLists.txt +++ b/gtsam/inference/CMakeLists.txt @@ -10,11 +10,11 @@ set(inference_excluded_files # Build tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(inference "${gtsam-default}" "${gtsam-default}" "${inference_excluded_files}") + gtsam_add_subdir_tests(inference "gtsam" "gtsam" "${inference_excluded_files}") endif(GTSAM_BUILD_TESTS) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(inference "${gtsam-default}" "${gtsam-default}" "${inference_excluded_files}") + gtsam_add_subdir_timing(inference "gtsam" "gtsam" "${inference_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt index aa1be1127..4a93e2875 100644 --- a/gtsam/linear/CMakeLists.txt +++ b/gtsam/linear/CMakeLists.txt @@ -10,7 +10,7 @@ set(linear_excluded_files # Build tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(linear "${gtsam-default}" "${gtsam-default}" "${linear_excluded_files}") + gtsam_add_subdir_tests(linear "gtsam" "gtsam" "${linear_excluded_files}") endif(GTSAM_BUILD_TESTS) if(MSVC) @@ -20,5 +20,5 @@ endif() # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(linear "${gtsam-default}" "${gtsam-default}" "${linear_excluded_files}") + gtsam_add_subdir_timing(linear "gtsam" "gtsam" "${linear_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp index 1ac1fbf46..ec7c01f88 100644 --- a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp @@ -114,19 +114,19 @@ TEST(GaussianBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree GaussianBayesTree bt; bt.insertRoot( - MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) + MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()), 2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished()), list_of - (MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0).finished()) + (MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0).finished()) (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished()) (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished()) (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()), 2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished()))) - (MakeClique(GaussianConditional(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0).finished()) + (MakeClique(GaussianConditional(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0).finished()) (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished()) (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()), 2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished()), list_of - (MakeClique(GaussianConditional(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0).finished()) + (MakeClique(GaussianConditional(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0).finished()) (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0).finished()) // NOTE the non-upper-triangular form // here since this test was written when we had column permutations @@ -136,11 +136,11 @@ TEST(GaussianBayesTree, complicatedMarginal) { (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished()) (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()), 2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished()), list_of - (MakeClique(GaussianConditional(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0).finished()) + (MakeClique(GaussianConditional(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0).finished()) (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished()) (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()), 2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished()))) - (MakeClique(GaussianConditional(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0).finished()) + (MakeClique(GaussianConditional(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0).finished()) (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished()) (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()), 2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished()))))))))); diff --git a/gtsam/navigation/CMakeLists.txt b/gtsam/navigation/CMakeLists.txt index f90f25f00..ed599cf15 100644 --- a/gtsam/navigation/CMakeLists.txt +++ b/gtsam/navigation/CMakeLists.txt @@ -7,10 +7,10 @@ set (navigation_excluded_tests "") # Add all tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(navigation "${gtsam-default}" "${gtsam-default}" "${navigation_excluded_tests}") + gtsam_add_subdir_tests(navigation "gtsam" "gtsam" "${navigation_excluded_tests}") endif() # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(navigation "${gtsam-default}" "${gtsam-default}" "${navigation_excluded_files}") + gtsam_add_subdir_timing(navigation "gtsam" "gtsam" "${navigation_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index f7e3cfec8..72d31b294 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -11,7 +11,7 @@ /** * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman **/ #pragma once @@ -342,11 +342,8 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ -#ifndef _MSC_VER - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif + typedef boost::shared_ptr shared_ptr; + /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index bd8a0f80b..0ec4c56e2 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,7 +11,7 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman **/ #pragma once @@ -304,11 +304,8 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ -#ifndef _MSC_VER - typedef typename boost::shared_ptr shared_ptr; -#else typedef boost::shared_ptr shared_ptr; -#endif + /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index 3e2fdb689..f4cf0b585 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -10,11 +10,11 @@ set(nonlinear_excluded_files # Build tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(nonlinear "${gtsam-default}" "${gtsam-default}" "${nonlinear_excluded_files}") + gtsam_add_subdir_tests(nonlinear "gtsam" "gtsam" "${nonlinear_excluded_files}") endif(GTSAM_BUILD_TESTS) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(nonlinear "${gtsam-default}" "${gtsam-default}" "${nonlinear_excluded_files}") + gtsam_add_subdir_timing(nonlinear "gtsam" "gtsam" "${nonlinear_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt index 53860d913..1c20e3720 100644 --- a/gtsam/slam/CMakeLists.txt +++ b/gtsam/slam/CMakeLists.txt @@ -15,10 +15,10 @@ set(slam_excluded_files # Build tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(slam "${gtsam-default}" "${gtsam-default}" "${slam_excluded_files}") + gtsam_add_subdir_tests(slam "gtsam" "gtsam" "${slam_excluded_files}") endif(GTSAM_BUILD_TESTS) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(slam "${gtsam-default}" "${gtsam-default}" "${slam_excluded_files}") + gtsam_add_subdir_timing(slam "gtsam" "gtsam" "${slam_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 6d85685d5..e7b3914f3 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -37,7 +37,7 @@ bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); PinholeCamera camera2(data.cameras[1].pose(),Cal3_S2()); -EssentialMatrix trueE(c1Rc2, c1Tc2); +EssentialMatrix trueE(c1Rc2, Sphere2(c1Tc2)); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { @@ -297,7 +297,7 @@ SfM_data data; bool readOK = readBAL(filename, data); Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); -EssentialMatrix trueE(aRb, aTb); +EssentialMatrix trueE(aRb, Sphere2(aTb)); double baseline = 10; // actual baseline of the camera diff --git a/gtsam/symbolic/CMakeLists.txt b/gtsam/symbolic/CMakeLists.txt index 9b92f40c6..a683f0711 100644 --- a/gtsam/symbolic/CMakeLists.txt +++ b/gtsam/symbolic/CMakeLists.txt @@ -10,11 +10,11 @@ set(symbolic_excluded_files # Build tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(symbolic "${gtsam-default}" "${gtsam-default}" "${symbolic_excluded_files}") + gtsam_add_subdir_tests(symbolic "gtsam" "gtsam" "${symbolic_excluded_files}") endif(GTSAM_BUILD_TESTS) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(symbolic "${gtsam-default}" "${gtsam-default}" "${symbolic_excluded_files}") + gtsam_add_subdir_timing(symbolic "gtsam" "gtsam" "${symbolic_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 72b6a97f6..4b7abdaba 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -58,40 +58,38 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") # build shared and static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM_UNSTABLE - static") - add_library(gtsam_unstable-static STATIC ${gtsam_unstable_srcs}) - set_target_properties(gtsam_unstable-static PROPERTIES + add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs}) + set_target_properties(gtsam_unstable PROPERTIES OUTPUT_NAME gtsam_unstable CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_unstable_version} SOVERSION ${gtsam_unstable_soversion}) if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library - set_target_properties(gtsam_unstable-static PROPERTIES + set_target_properties(gtsam_unstable PROPERTIES PREFIX "lib" COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC) endif() - target_link_libraries(gtsam_unstable-static gtsam-static ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) - install(TARGETS gtsam_unstable-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib) - list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-static) + target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) + install(TARGETS gtsam_unstable EXPORT GTSAM-exports ARCHIVE DESTINATION lib) + list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -endif() - -if (GTSAM_BUILD_SHARED_LIBRARY) +else() message(STATUS "Building GTSAM_UNSTABLE - shared") - add_library(gtsam_unstable-shared SHARED ${gtsam_unstable_srcs}) - set_target_properties(gtsam_unstable-shared PROPERTIES + add_library(gtsam_unstable SHARED ${gtsam_unstable_srcs}) + set_target_properties(gtsam_unstable PROPERTIES OUTPUT_NAME gtsam_unstable CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_unstable_version} SOVERSION ${gtsam_unstable_soversion}) if(WIN32) - set_target_properties(gtsam_unstable-shared PROPERTIES + set_target_properties(gtsam_unstable PROPERTIES PREFIX "" DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") endif() - target_link_libraries(gtsam_unstable-shared gtsam-shared ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) - install(TARGETS gtsam_unstable-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) - list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-shared) + target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) + install(TARGETS gtsam_unstable EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) + list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) endif() @@ -102,12 +100,12 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Generate, build and install toolbox set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") - if("${gtsam-default}" STREQUAL "gtsam-static") + if(GTSAM_BUILD_STATIC_LIBRARY) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) endif() # Wrap - wrap_and_install_library(gtsam_unstable.h "${gtsam-default};${gtsam_unstable-default}" "" "${mexFlags}") + wrap_and_install_library(gtsam_unstable.h "gtsam;gtsam_unstable" "" "${mexFlags}") endif(GTSAM_INSTALL_MATLAB_TOOLBOX) diff --git a/gtsam_unstable/base/CMakeLists.txt b/gtsam_unstable/base/CMakeLists.txt index 40337f6a6..22737d533 100644 --- a/gtsam_unstable/base/CMakeLists.txt +++ b/gtsam_unstable/base/CMakeLists.txt @@ -3,8 +3,8 @@ file(GLOB base_headers "*.h") install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base) set (base_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) + gtsam + gtsam_unstable) # Exclude tests that don't work set (base_excluded_tests "") diff --git a/gtsam_unstable/discrete/CMakeLists.txt b/gtsam_unstable/discrete/CMakeLists.txt index c978fcde5..f6cfcb1a8 100644 --- a/gtsam_unstable/discrete/CMakeLists.txt +++ b/gtsam_unstable/discrete/CMakeLists.txt @@ -3,8 +3,8 @@ file(GLOB discrete_headers "*.h") install(FILES ${discrete_headers} DESTINATION include/gtsam_unstable/discrete) set (discrete_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) + gtsam + gtsam_unstable) # Exclude tests that don't work #set (discrete_excluded_tests @@ -32,11 +32,11 @@ if (GTSAM_BUILD_EXAMPLES) set_target_properties(${example} PROPERTIES EXCLUDE_FROM_ALL ON) endif() - if(NOT MSVC) + if(NOT MSVC AND NOT XCODE_VERSION) add_dependencies(examples ${example}) add_custom_target(${example}.run ${EXECUTABLE_OUTPUT_PATH}${example} ${ARGN}) endif() - target_link_libraries(${example} ${gtsam-default} ${gtsam_unstable-default}) + target_link_libraries(${example} gtsam gtsam_unstable) endforeach(example) endif (GTSAM_BUILD_EXAMPLES) diff --git a/gtsam_unstable/dynamics/CMakeLists.txt b/gtsam_unstable/dynamics/CMakeLists.txt index a05c9ef26..382b2b4cf 100644 --- a/gtsam_unstable/dynamics/CMakeLists.txt +++ b/gtsam_unstable/dynamics/CMakeLists.txt @@ -4,8 +4,8 @@ install(FILES ${dynamics_headers} DESTINATION include/gtsam_unstable/dynamics) # Components to link tests in this subfolder against set (dynamics_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) + gtsam + gtsam_unstable) # Exclude tests that don't work set (dynamics_excluded_tests "") diff --git a/gtsam_unstable/examples/CMakeLists.txt b/gtsam_unstable/examples/CMakeLists.txt index 7af84a784..d95ab7706 100644 --- a/gtsam_unstable/examples/CMakeLists.txt +++ b/gtsam_unstable/examples/CMakeLists.txt @@ -18,8 +18,8 @@ foreach(example_src ${example_srcs} ) set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON) endif() - target_link_libraries(${example_bin} ${gtsam-default} ${gtsam_unstable-default}) - if(NOT MSVC) + target_link_libraries(${example_bin} gtsam gtsam_unstable) + if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) endif() diff --git a/gtsam_unstable/geometry/CMakeLists.txt b/gtsam_unstable/geometry/CMakeLists.txt index bea2a51f0..c7ee3902f 100644 --- a/gtsam_unstable/geometry/CMakeLists.txt +++ b/gtsam_unstable/geometry/CMakeLists.txt @@ -4,8 +4,8 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry) # Components to link tests in this subfolder against set (geometry_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) + gtsam + gtsam_unstable) # Exclude tests that don't work set (geometry_excluded_tests "") diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index c4ad43a21..80ee41b22 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -352,6 +352,20 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor { void serializable() const; // enabling serialization functionality }; +#include +template +virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor { + TransformBtwRobotsUnaryFactor(size_t key, const T& relativePose, size_t keyA, size_t keyB, + const gtsam::Values& valA, const gtsam::Values& valB, + const gtsam::noiseModel::Gaussian* model); + + Vector whitenedError(const gtsam::Values& x); + Vector unwhitenedError(const gtsam::Values& x); + void setValAValB(const gtsam::Values valA, const gtsam::Values valB); + + void serializable() const; // enabling serialization functionality +}; + #include virtual class SmartRangeFactor : gtsam::NoiseModelFactor { SmartRangeFactor(double s); diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt index b4f386065..daef8983c 100644 --- a/gtsam_unstable/nonlinear/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/CMakeLists.txt @@ -4,8 +4,8 @@ install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear) # Components to link tests in this subfolder against set (nonlinear_full_libs -${gtsam-default} -${gtsam_unstable-default}) +gtsam +gtsam_unstable) # Exclude tests that don't work set (nonlinear_excluded_tests #"") diff --git a/gtsam_unstable/slam/CMakeLists.txt b/gtsam_unstable/slam/CMakeLists.txt index a0229de19..ae4784f70 100644 --- a/gtsam_unstable/slam/CMakeLists.txt +++ b/gtsam_unstable/slam/CMakeLists.txt @@ -9,8 +9,8 @@ install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam) # Components to link tests in this subfolder against set (slam_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) + gtsam + gtsam_unstable) # Exclude tests that don't work set (slam_excluded_tests diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h new file mode 100644 index 000000000..c3564a748 --- /dev/null +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -0,0 +1,245 @@ +/* ---------------------------------------------------------------------------- + + * 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 TransformBtwRobotsUnaryFactor.h + * @brief Unary factor for determining transformation between given trajectories of two robots + * @author Vadim Indelman + **/ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * @tparam VALUE the Value type + * @addtogroup SLAM + */ + template + class TransformBtwRobotsUnaryFactor: public NonlinearFactor { + + public: + + typedef VALUE T; + + private: + + typedef TransformBtwRobotsUnaryFactor This; + typedef gtsam::NonlinearFactor Base; + + gtsam::Key key_; + + VALUE measured_; /** The measurement */ + + gtsam::Values valA_; // given values for robot A map\trajectory + gtsam::Values valB_; // given values for robot B map\trajectory + gtsam::Key keyA_; // key of robot A to which the measurement refers + gtsam::Key keyB_; // key of robot B to which the measurement refers + + SharedGaussian model_; + + /** concept check by type */ + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + TransformBtwRobotsUnaryFactor() {} + + /** Constructor */ + TransformBtwRobotsUnaryFactor(Key key, const VALUE& measured, Key keyA, Key keyB, + const gtsam::Values valA, const gtsam::Values valB, + const SharedGaussian& model) : + Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), + model_(model){ + + setValAValB(valA, valB); + + } + + virtual ~TransformBtwRobotsUnaryFactor() {} + + + /** Clone */ + virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared(*this); } + + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "TransformBtwRobotsUnaryFactor(" + << keyFormatter(key_) << ")\n"; + std::cout << "MR between factor keys: " + << keyFormatter(keyA_) << "," + << keyFormatter(keyB_) << "\n"; + measured_.print(" measured: "); + model_->print(" noise model: "); + // Base::print(s, keyFormatter); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const { + const This *t = dynamic_cast (&f); + + if(t && Base::equals(f)) + return key_ == t->key_ && measured_.equals(t->measured_); + else + return false; + } + + /** implement functions needed to derive from Factor */ + + /* ************************************************************************* */ + void setValAValB(const gtsam::Values valA, const gtsam::Values valB){ + if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) ) + throw("something is wrong!"); + + // TODO: make sure the two keys belong to different robots + + if (valA.exists(keyA_)){ + valA_ = valA; + valB_ = valB; + } + else { + valA_ = valB; + valB_ = valA; + } + } + + /* ************************************************************************* */ + virtual double error(const gtsam::Values& x) const { + return whitenedError(x).squaredNorm(); + } + + /* ************************************************************************* */ + /** + * Linearize a non-linearFactorN to get a gtsam::GaussianFactor, + * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ + * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ + */ + /* This version of linearize recalculates the noise model each time */ + virtual boost::shared_ptr linearize(const gtsam::Values& x) const { + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); + + //std::cout<<"About to linearize"< A(this->size()); + gtsam::Vector b = -whitenedError(x, A); + A1 = A[0]; + + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size()))); + } + + + /* ************************************************************************* */ + gtsam::Vector whitenedError(const gtsam::Values& x, + boost::optional&> H = boost::none) const { + + bool debug = true; + + Matrix H_compose, H_between1, H_dummy; + + T orgA_T_currA = valA_.at(keyA_); + T orgB_T_currB = valB_.at(keyB_); + + T orgA_T_orgB = x.at(key_); + + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy); + + T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1); + + T currA_T_currB_msr = measured_; + + Vector err_unw = currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + + Vector err_wh = err_unw; + if (H) { + (*H)[0] = H_compose * H_between1; + model_->WhitenSystem(*H, err_wh); + } + else { + model_->whitenInPlace(err_wh); + } + + Vector err_wh2 = model_->whiten(err_wh); + + if (debug){ + // std::cout<<"err_wh: "<(keyA_); + T orgB_T_currB = valB_.at(keyB_); + + T orgA_T_orgB = x.at(key_); + + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB); + + T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB); + + T currA_T_currB_msr = measured_; + + return currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + } + + /* ************************************************************************* */ + + /** number of variables attached to this factor */ + std::size_t size() const { + return 1; + } + + virtual size_t dim() const { + return model_->R().rows() + model_->R().cols(); + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + //ar & BOOST_SERIALIZATION_NVP(measured_); + } + }; // \class TransformBtwRobotsUnaryFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp new file mode 100644 index 000000000..e20d8bbcf --- /dev/null +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -0,0 +1,317 @@ +/** + * @file testBTransformBtwRobotsUnaryFactor.cpp + * @brief Unit test for the TransformBtwRobotsUnaryFactor + * @author Vadim Indelman + */ + +#include + + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +//#include + + +using namespace std; +using namespace gtsam; + +// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below +// to reenable the test. +//#if 0 +/* ************************************************************************* */ +LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor& factor){ + gtsam::Values values; + values.insert(key, org1_T_org2); + // LieVector err = factor.whitenedError(values); + // return err; + return LieVector::Expmap(factor.whitenedError(values)); +} + +/* ************************************************************************* */ +//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ +// gtsam::Values values; +// values.insert(keyA, p1); +// values.insert(keyB, p2); +// // LieVector err = factor.whitenedError(values); +// // return err; +// return LieVector::Expmap(factor.whitenedError(values)); +//} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, ConstructorAndEquals) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 p1(10.0, 15.0, 0.1); + gtsam::Pose2 p2(15.0, 15.0, 0.3); + gtsam::Pose2 noise(0.5, 0.4, 0.01); + gtsam::Pose2 rel_pose_ideal = p1.between(p2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, p1); + valB.insert(keyB, p2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + TransformBtwRobotsUnaryFactor h(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + // Equals + CHECK(assert_equal(g, h, 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, unwhitenedError) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1); + gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3); + + gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8); + + gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2); + + gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0); + gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1); + + gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0); + + gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB); + + gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + double prior_outlier = 0.01; + double prior_inlier = 0.99; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, Optimize) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0); + gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05); + + gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0); + gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(orgB_T_currB); + gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(orgA_T_currB_tr); + + // some error in measurements + // gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01)); + + // ideal measurements + gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.0, 0.0, 0.0)); + gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1; + gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; + gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactor g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, gtsam::Pose2()); + + gtsam::NonlinearFactorGraph graph; + graph.push_back(g1); + graph.push_back(g2); + graph.push_back(g3); + graph.push_back(g4); + + gtsam::GaussNewtonParams params; + gtsam::GaussNewtonOptimizer optimizer(graph, values, params); + gtsam::Values result = optimizer.optimize(); + + gtsam::Pose2 orgA_T_orgB_opt = result.at(key); + + CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5)); +} + + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, Jacobian) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1); + gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3); + + gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8); + + gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2); + + gtsam::Pose2 noise(0.5, 0.4, 0.01); + + gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + + std::vector H_actual(1); + Vector actual_err_wh = g.whitenedError(values, H_actual); + + Matrix H1_actual = H_actual[0]; + + double stepsize = 1.0e-9; + Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); +// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); +} + + + + +/////* ************************************************************************** */ +//TEST (TransformBtwRobotsUnaryFactor, jacobian ) { +// +// gtsam::Key keyA(1); +// gtsam::Key keyB(2); +// +// // Inlier test +// gtsam::Pose2 p1(10.0, 15.0, 0.1); +// gtsam::Pose2 p2(15.0, 15.0, 0.3); +// gtsam::Pose2 noise(0.5, 0.4, 0.01); +// gtsam::Pose2 rel_pose_ideal = p1.between(p2); +// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); +// +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); +// +// gtsam::Values values; +// values.insert(keyA, p1); +// values.insert(keyB, p2); +// +// double prior_outlier = 0.0; +// double prior_inlier = 1.0; +// +// TransformBtwRobotsUnaryFactor f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier, +// prior_inlier, prior_outlier); +// +// std::vector H_actual(2); +// Vector actual_err_wh = f.whitenedError(values, H_actual); +// +// Matrix H1_actual = H_actual[0]; +// Matrix H2_actual = H_actual[1]; +// +// // compare to standard between factor +// BetweenFactor h(keyA, keyB, rel_pose_msr, model_inlier ); +// Vector actual_err_wh_stnd = h.whitenedError(values); +// Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); +// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); +// std::vector H_actual_stnd_unwh(2); +// (void)h.unwhitenedError(values, H_actual_stnd_unwh); +// Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0]; +// Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1]; +// Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh); +// Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh); +//// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8)); +//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); +// +// double stepsize = 1.0e-9; +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// +// +// // try to check numerical derivatives of a standard between factor +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); +// +// +// CHECK( assert_equal(H1_expected, H1_actual, 1e-8)); +// CHECK( assert_equal(H2_expected, H2_actual, 1e-8)); +// +//} + +//#endif + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index da8387614..a6ae2afca 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,6 +1,6 @@ # Assemble local libraries set (tests_full_libs - ${gtsam-default} + gtsam CppUnitLite) # exclude certain files