Merge branch 'develop' into feature/system-metis-lib
						commit
						8c7a404ace
					
				|  | @ -66,6 +66,8 @@ function configure() | |||
|       -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ | ||||
|       -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \ | ||||
|       -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ | ||||
|       -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ | ||||
|       -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ | ||||
|       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ | ||||
|       -DBOOST_ROOT=$BOOST_ROOT \ | ||||
|       -DBoost_NO_SYSTEM_PATHS=ON \ | ||||
|  |  | |||
|  | @ -63,17 +63,17 @@ jobs: | |||
| 
 | ||||
|           sudo apt install cmake build-essential pkg-config libpython-dev python-numpy | ||||
|            | ||||
|           echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)" | ||||
|           echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)" | ||||
|           echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV | ||||
|           echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV | ||||
|            | ||||
|           if [ "${{ matrix.compiler }}" = "gcc" ]; then | ||||
|             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib | ||||
|             echo "::set-env name=CC::gcc-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::g++-${{ matrix.version }}" | ||||
|             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           else | ||||
|             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib | ||||
|             echo "::set-env name=CC::clang-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::clang++-${{ matrix.version }}" | ||||
|             echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           fi | ||||
|       - name: Check Boost version | ||||
|         if: runner.os == 'Linux' | ||||
|  |  | |||
|  | @ -35,17 +35,19 @@ jobs: | |||
|       - name: Install (macOS) | ||||
|         if: runner.os == 'macOS' | ||||
|         run: | | ||||
|           brew install cmake ninja boost | ||||
|           brew tap ProfFan/robotics | ||||
|           brew install cmake ninja | ||||
|           brew install ProfFan/robotics/boost | ||||
|           if [ "${{ matrix.compiler }}" = "gcc" ]; then | ||||
|             brew install gcc@${{ matrix.version }} | ||||
|             echo "::set-env name=CC::gcc-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::g++-${{ matrix.version }}" | ||||
|             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           else | ||||
|             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app | ||||
|             echo "::set-env name=CC::clang" | ||||
|             echo "::set-env name=CXX::clang++" | ||||
|             echo "CC=clang" >> $GITHUB_ENV | ||||
|             echo "CXX=clang++" >> $GITHUB_ENV | ||||
|           fi | ||||
|       - name: Build and Test (macOS) | ||||
|         if: runner.os == 'macOS' | ||||
|         run: | | ||||
|           bash .github/scripts/unix.sh -t | ||||
|           bash .github/scripts/unix.sh -t | ||||
|  |  | |||
|  | @ -19,7 +19,7 @@ jobs: | |||
|         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. | ||||
|         name: [ | ||||
|           ubuntu-18.04-gcc-5, | ||||
|           # ubuntu-18.04-gcc-9,  # TODO Disabled for now because of timeouts | ||||
|           ubuntu-18.04-gcc-9, | ||||
|           ubuntu-18.04-clang-9, | ||||
|           macOS-10.15-xcode-11.3.1, | ||||
|           ubuntu-18.04-gcc-5-tbb, | ||||
|  | @ -33,11 +33,10 @@ jobs: | |||
|             compiler: gcc | ||||
|             version: "5" | ||||
| 
 | ||||
|           # TODO Disabled for now because of timeouts | ||||
|           # - name: ubuntu-18.04-gcc-9 | ||||
|           #   os: ubuntu-18.04 | ||||
|           #   compiler: gcc | ||||
|           #   version: "9" | ||||
|           - name: ubuntu-18.04-gcc-9 | ||||
|             os: ubuntu-18.04 | ||||
|             compiler: gcc | ||||
|             version: "9" | ||||
| 
 | ||||
|           - name: ubuntu-18.04-clang-9 | ||||
|             os: ubuntu-18.04 | ||||
|  | @ -77,30 +76,32 @@ jobs: | |||
|            | ||||
|           if [ "${{ matrix.compiler }}" = "gcc" ]; then | ||||
|             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib | ||||
|             echo "::set-env name=CC::gcc-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::g++-${{ matrix.version }}" | ||||
|             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           else | ||||
|             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib | ||||
|             echo "::set-env name=CC::clang-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::clang++-${{ matrix.version }}" | ||||
|             echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           fi | ||||
|       - name: Install (macOS) | ||||
|         if: runner.os == 'macOS' | ||||
|         run: | | ||||
|           brew install cmake ninja boost | ||||
|           brew tap ProfFan/robotics | ||||
|           brew install cmake ninja | ||||
|           brew install ProfFan/robotics/boost | ||||
|           if [ "${{ matrix.compiler }}" = "gcc" ]; then | ||||
|             brew install gcc@${{ matrix.version }} | ||||
|             echo "::set-env name=CC::gcc-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::g++-${{ matrix.version }}" | ||||
|             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           else | ||||
|             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app | ||||
|             echo "::set-env name=CC::clang" | ||||
|             echo "::set-env name=CXX::clang++" | ||||
|             echo "CC=clang" >> $GITHUB_ENV | ||||
|             echo "CXX=clang++" >> $GITHUB_ENV | ||||
|           fi | ||||
|       - name: Set GTSAM_WITH_TBB Flag | ||||
|         if: matrix.flag == 'tbb' | ||||
|         run: | | ||||
|           echo "::set-env name=GTSAM_WITH_TBB::ON" | ||||
|           echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV | ||||
|           echo "GTSAM Uses TBB" | ||||
|       - name: Build (Linux) | ||||
|         if: runner.os == 'Linux' | ||||
|  | @ -109,4 +110,4 @@ jobs: | |||
|       - name: Build (macOS) | ||||
|         if: runner.os == 'macOS' | ||||
|         run: | | ||||
|           bash .github/scripts/python.sh | ||||
|           bash .github/scripts/python.sh | ||||
|  |  | |||
|  | @ -24,6 +24,7 @@ jobs: | |||
|             ubuntu-gcc-deprecated, | ||||
|             ubuntu-gcc-quaternions, | ||||
|             ubuntu-gcc-tbb, | ||||
|             ubuntu-cayleymap, | ||||
|           ] | ||||
| 
 | ||||
|         build_type: [Debug, Release] | ||||
|  | @ -47,6 +48,12 @@ jobs: | |||
|             version: "9" | ||||
|             flag: tbb | ||||
| 
 | ||||
|           - name: ubuntu-cayleymap | ||||
|             os: ubuntu-18.04 | ||||
|             compiler: gcc | ||||
|             version: "9" | ||||
|             flag: cayley | ||||
| 
 | ||||
|     steps: | ||||
|       - name: Checkout | ||||
|         uses: actions/checkout@master | ||||
|  | @ -64,17 +71,17 @@ jobs: | |||
| 
 | ||||
|           sudo apt install cmake build-essential pkg-config libpython-dev python-numpy | ||||
| 
 | ||||
|           echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)" | ||||
|           echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)" | ||||
|           echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV | ||||
|           echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV | ||||
| 
 | ||||
|           if [ "${{ matrix.compiler }}" = "gcc" ]; then | ||||
|             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib | ||||
|             echo "::set-env name=CC::gcc-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::g++-${{ matrix.version }}" | ||||
|             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           else | ||||
|             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib | ||||
|             echo "::set-env name=CC::clang-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::clang++-${{ matrix.version }}" | ||||
|             echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           fi | ||||
| 
 | ||||
|       - name: Install (macOS) | ||||
|  | @ -83,32 +90,39 @@ jobs: | |||
|           brew install cmake ninja boost | ||||
|           if [ "${{ matrix.compiler }}" = "gcc" ]; then | ||||
|             brew install gcc@${{ matrix.version }} | ||||
|             echo "::set-env name=CC::gcc-${{ matrix.version }}" | ||||
|             echo "::set-env name=CXX::g++-${{ matrix.version }}" | ||||
|             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV | ||||
|           else | ||||
|             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app | ||||
|             echo "::set-env name=CC::clang" | ||||
|             echo "::set-env name=CXX::clang++" | ||||
|             echo "CC=clang" >> $GITHUB_ENV | ||||
|             echo "CXX=clang++" >> $GITHUB_ENV | ||||
|             fi | ||||
| 
 | ||||
|       - name: Set Allow Deprecated Flag | ||||
|         if: matrix.flag == 'deprecated' | ||||
|         run: | | ||||
|           echo "::set-env name=GTSAM_ALLOW_DEPRECATED_SINCE_V41::ON" | ||||
|           echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV | ||||
|           echo "Allow deprecated since version 4.1" | ||||
| 
 | ||||
|       - name: Set Use Quaternions Flag | ||||
|         if: matrix.flag == 'quaternions' | ||||
|         run: | | ||||
|           echo "::set-env name=GTSAM_USE_QUATERNIONS::ON" | ||||
|           echo "GTSAM_USE_QUATERNIONS=ON" >> $GITHUB_ENV | ||||
|           echo "Use Quaternions for rotations" | ||||
| 
 | ||||
|       - name: Set GTSAM_WITH_TBB Flag | ||||
|         if: matrix.flag == 'tbb' | ||||
|         run: | | ||||
|           echo "::set-env name=GTSAM_WITH_TBB::ON" | ||||
|           echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV | ||||
|           echo "GTSAM Uses TBB" | ||||
| 
 | ||||
|       - name: Use Cayley Transform for Rot3 | ||||
|         if: matrix.flag == 'cayley' | ||||
|         run: | | ||||
|           echo "GTSAM_POSE3_EXPMAP=OFF" >> $GITHUB_ENV | ||||
|           echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV | ||||
|           echo "GTSAM Uses Cayley map for Rot3" | ||||
| 
 | ||||
|       - name: Build & Test | ||||
|         run: | | ||||
|           bash .github/scripts/unix.sh -t | ||||
|  |  | |||
|  | @ -18,16 +18,19 @@ jobs: | |||
|         # Github Actions requires a single row to be added to the build matrix. | ||||
|         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. | ||||
|         name: [ | ||||
|           windows-2016-cl, | ||||
|           #TODO This build keeps timing out, need to understand why. | ||||
|           # windows-2016-cl, | ||||
|           windows-2019-cl, | ||||
|         ] | ||||
| 
 | ||||
|         build_type: [Debug, Release] | ||||
|         build_unstable: [ON] | ||||
|         include: | ||||
|           - name: windows-2016-cl | ||||
|             os: windows-2016 | ||||
|             compiler: cl | ||||
| 
 | ||||
|           #TODO This build keeps timing out, need to understand why. | ||||
|           # - name: windows-2016-cl | ||||
|           #   os: windows-2016 | ||||
|           #   compiler: cl | ||||
| 
 | ||||
|           - name: windows-2019-cl | ||||
|             os: windows-2019 | ||||
|  | @ -50,17 +53,17 @@ jobs: | |||
|             # See: https://github.com/DaanDeMeyer/doctest/runs/231595515 | ||||
|             # See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413 | ||||
|             scoop install gcc --global | ||||
|             echo "::set-env name=CC::gcc" | ||||
|             echo "::set-env name=CXX::g++" | ||||
|             echo "CC=gcc" >> $GITHUB_ENV | ||||
|             echo "CXX=g++" >> $GITHUB_ENV | ||||
|           } elseif ("${{ matrix.compiler }}" -eq "clang") { | ||||
|             echo "::set-env name=CC::clang" | ||||
|             echo "::set-env name=CXX::clang++" | ||||
|             echo "CC=clang" >> $GITHUB_ENV | ||||
|             echo "CXX=clang++" >> $GITHUB_ENV | ||||
|           } else { | ||||
|             echo "::set-env name=CC::${{ matrix.compiler }}" | ||||
|             echo "::set-env name=CXX::${{ matrix.compiler }}" | ||||
|             echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV | ||||
|             echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV | ||||
|           } | ||||
|           # Scoop modifies the PATH so we make the modified PATH global. | ||||
|           echo "::set-env name=PATH::$env:PATH" | ||||
|           echo "$env:PATH" >> $GITHUB_PATH | ||||
|       - name: Build (Windows) | ||||
|         if: runner.os == 'Windows' | ||||
|         run: | | ||||
|  |  | |||
|  | @ -1,3 +1,5 @@ | |||
| include(CheckCXXCompilerFlag) # for check_cxx_compiler_flag() | ||||
| 
 | ||||
| # Set cmake policy to recognize the AppleClang compiler | ||||
| # independently from the Clang compiler. | ||||
| if(POLICY CMP0025) | ||||
|  | @ -105,11 +107,14 @@ if(MSVC) | |||
| else() | ||||
|   # Common to all configurations, next for each configuration: | ||||
| 
 | ||||
|   if ( | ||||
|       ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR | ||||
|       (CMAKE_CXX_COMPILER_ID MATCHES "GNU") | ||||
|      ) | ||||
|     set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday | ||||
|   if (NOT MSVC) | ||||
|     check_cxx_compiler_flag(-Wsuggest-override COMPILER_HAS_WSUGGEST_OVERRIDE) | ||||
|     check_cxx_compiler_flag(-Wmissing COMPILER_HAS_WMISSING_OVERRIDE) | ||||
|     if (COMPILER_HAS_WSUGGEST_OVERRIDE) | ||||
|       set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday | ||||
|     elseif(COMPILER_HAS_WMISSING_OVERRIDE) | ||||
|       set(flag_override_ -Wmissing-override) # -Werror=missing-override: Add again someday | ||||
|     endif() | ||||
|   endif() | ||||
| 
 | ||||
|   set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON | ||||
|  |  | |||
|  | @ -31,6 +31,15 @@ if(NOT MSVC AND NOT XCODE_VERSION) | |||
|     option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON) | ||||
| endif() | ||||
| 
 | ||||
| # Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa. | ||||
| if(GTSAM_POSE3_EXPMAP) | ||||
|     message(STATUS "GTSAM_POSE3_EXPMAP=ON, enabling GTSAM_ROT3_EXPMAP as well") | ||||
|     set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE) | ||||
| elseif(GTSAM_ROT3_EXPMAP) | ||||
|     message(STATUS "GTSAM_ROT3_EXPMAP=ON, enabling GTSAM_POSE3_EXPMAP as well") | ||||
|     set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE) | ||||
| endif() | ||||
| 
 | ||||
| # Options relating to MATLAB wrapper | ||||
| # TODO: Check for matlab mex binary before handling building of binaries | ||||
| option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF) | ||||
|  |  | |||
|  | @ -1,4 +1,5 @@ | |||
| if(GTSAM_BUILD_PYTHON) | ||||
| # Set Python version if either Python or MATLAB wrapper is requested. | ||||
| if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) | ||||
|     if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") | ||||
|         # Get info about the Python3 interpreter | ||||
|         # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 | ||||
|  | @ -14,7 +15,9 @@ if(GTSAM_BUILD_PYTHON) | |||
|                 "The version of Python to build the wrappers against." | ||||
|                 FORCE) | ||||
|     endif() | ||||
| endif() | ||||
| 
 | ||||
| if(GTSAM_BUILD_PYTHON) | ||||
|     if(GTSAM_UNSTABLE_BUILD_PYTHON) | ||||
|         if (NOT GTSAM_BUILD_UNSTABLE) | ||||
|             message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") | ||||
|  |  | |||
|  | @ -1188,7 +1188,7 @@ USE_MATHJAX            = YES | |||
| # MathJax, but it is strongly recommended to install a local copy of MathJax  | ||||
| # before deployment. | ||||
| 
 | ||||
| MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest | ||||
| MATHJAX_RELPATH        = https://cdn.mathjax.org/mathjax/latest | ||||
| 
 | ||||
| # The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension  | ||||
| # names that should be enabled during MathJax rendering. | ||||
|  |  | |||
|  | @ -23,6 +23,7 @@ | |||
| #include <boost/optional.hpp> | ||||
| #include <map> | ||||
| #include <iostream> | ||||
| #include <sstream> | ||||
| #include <vector> | ||||
| 
 | ||||
| namespace gtsam { | ||||
|  | @ -349,4 +350,44 @@ bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) { | |||
|   return false; | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Capture std out via cout stream and compare against string. | ||||
|  */ | ||||
| template<class V> | ||||
| bool assert_stdout_equal(const std::string& expected, const V& actual) { | ||||
|   // Redirect output to buffer so we can compare
 | ||||
|   std::stringstream buffer; | ||||
|   // Save the original output stream so we can reset later
 | ||||
|   std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); | ||||
| 
 | ||||
|   // We test against actual std::cout for faithful reproduction
 | ||||
|   std::cout << actual; | ||||
| 
 | ||||
|   // Get output string and reset stdout
 | ||||
|   std::string actual_ = buffer.str(); | ||||
|   std::cout.rdbuf(old); | ||||
| 
 | ||||
|   return assert_equal(expected, actual_); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Capture print function output and compare against string. | ||||
|  */ | ||||
| template<class V> | ||||
| bool assert_print_equal(const std::string& expected, const V& actual) { | ||||
|   // Redirect output to buffer so we can compare
 | ||||
|   std::stringstream buffer; | ||||
|   // Save the original output stream so we can reset later
 | ||||
|   std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); | ||||
| 
 | ||||
|   // We test against actual std::cout for faithful reproduction
 | ||||
|   actual.print(); | ||||
| 
 | ||||
|   // Get output string and reset stdout
 | ||||
|   std::string actual_ = buffer.str(); | ||||
|   std::cout.rdbuf(old); | ||||
| 
 | ||||
|   return assert_equal(expected, actual_); | ||||
| } | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
|  |  | |||
|  | @ -98,6 +98,17 @@ public: | |||
|     return k2_; | ||||
|   } | ||||
| 
 | ||||
|   /// image center in x
 | ||||
|   inline double px() const { | ||||
|     return u0_; | ||||
|   } | ||||
| 
 | ||||
|   /// image center in y
 | ||||
|   inline double py() const { | ||||
|     return v0_; | ||||
|   } | ||||
| 
 | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 | ||||
|   /// get parameter u0
 | ||||
|   inline double u0() const { | ||||
|     return u0_; | ||||
|  | @ -107,6 +118,7 @@ public: | |||
|   inline double v0() const { | ||||
|     return v0_; | ||||
|   } | ||||
| #endif | ||||
| 
 | ||||
| 
 | ||||
|   /**
 | ||||
|  |  | |||
|  | @ -526,7 +526,7 @@ namespace gtsam { | |||
| 
 | ||||
|     /**
 | ||||
|      * @brief Spherical Linear intERPolation between *this and other | ||||
|      * @param s a value between 0 and 1 | ||||
|      * @param t a value between 0 and 1 | ||||
|      * @param other final point of iterpolation geodesic on manifold | ||||
|      */ | ||||
|     Rot3 slerp(double t, const Rot3& other) const; | ||||
|  |  | |||
|  | @ -176,7 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { | |||
|   if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); | ||||
|   // Create a fixed-size matrix
 | ||||
|   Matrix3 A = R.matrix(); | ||||
|   // Mathematica closed form optimization (procrastination?) gone wild:
 | ||||
| 
 | ||||
|   // Check if (A+I) is invertible. Same as checking for -1 eigenvalue.
 | ||||
|   if ((A + I_3x3).determinant() == 0.0) { | ||||
|     throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); | ||||
|   } | ||||
| 
 | ||||
|   // Mathematica closed form optimization.
 | ||||
|   // The following are the essential computations for the following algorithm
 | ||||
|   // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 
 | ||||
|   // 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I)
 | ||||
|   // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z.
 | ||||
|   const double a = A(0, 0), b = A(0, 1), c = A(0, 2); | ||||
|   const double d = A(1, 0), e = A(1, 1), f = A(1, 2); | ||||
|   const double g = A(2, 0), h = A(2, 1), i = A(2, 2); | ||||
|  |  | |||
|  | @ -16,6 +16,7 @@ | |||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/geometry/Cal3_S2.h> | ||||
| 
 | ||||
|  | @ -127,6 +128,16 @@ TEST(Cal3_S2, between) { | |||
| 
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Cal3_S2, Print) { | ||||
|   Cal3_S2 cal(5, 5, 5, 5, 5); | ||||
|   std::stringstream os; | ||||
|   os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px() | ||||
|      << ", py:" << cal.py() << "}"; | ||||
| 
 | ||||
|   EXPECT(assert_stdout_equal(os.str(), cal)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|  |  | |||
|  | @ -18,6 +18,7 @@ | |||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/base/testLie.h> | ||||
| #include <gtsam/base/lieProxies.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| 
 | ||||
| #include <boost/assign/std/vector.hpp> // for operator += | ||||
| using namespace boost::assign; | ||||
|  | @ -906,9 +907,9 @@ TEST(Pose3 , ChartDerivatives) { | |||
|   Pose3 id; | ||||
|   if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { | ||||
|     CHECK_CHART_DERIVATIVES(id,id); | ||||
| //    CHECK_CHART_DERIVATIVES(id,T2);
 | ||||
| //    CHECK_CHART_DERIVATIVES(T2,id);
 | ||||
| //    CHECK_CHART_DERIVATIVES(T2,T3);
 | ||||
|     CHECK_CHART_DERIVATIVES(id,T2); | ||||
|     CHECK_CHART_DERIVATIVES(T2,id); | ||||
|     CHECK_CHART_DERIVATIVES(T2,T3); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
|  | @ -1028,32 +1029,13 @@ TEST(Pose3, Create) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Pose3, print) { | ||||
|   std::stringstream redirectStream; | ||||
|   std::streambuf* ssbuf = redirectStream.rdbuf(); | ||||
|   std::streambuf* oldbuf  = std::cout.rdbuf(); | ||||
|   // redirect cout to redirectStream
 | ||||
|   std::cout.rdbuf(ssbuf); | ||||
| 
 | ||||
| TEST(Pose3, Print) { | ||||
|   Pose3 pose(Rot3::identity(), Point3(1, 2, 3)); | ||||
|   // output is captured to redirectStream
 | ||||
|   pose.print(); | ||||
| 
 | ||||
|   // Generate the expected output
 | ||||
|   std::stringstream expected; | ||||
|   Point3 translation(1, 2, 3); | ||||
|   std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n"; | ||||
| 
 | ||||
|   // Add expected rotation
 | ||||
|   expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; | ||||
|   expected << "t: 1 2 3\n"; | ||||
| 
 | ||||
|   // reset cout to the original stream
 | ||||
|   std::cout.rdbuf(oldbuf); | ||||
| 
 | ||||
|   // Get substring corresponding to translation part
 | ||||
|   std::string actual = redirectStream.str(); | ||||
| 
 | ||||
|   CHECK_EQUAL(expected.str(), actual); | ||||
|   EXPECT(assert_print_equal(expected, pose)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -597,6 +597,7 @@ class Rot3 { | |||
|   Rot3(double R11, double R12, double R13, | ||||
|       double R21, double R22, double R23, | ||||
|       double R31, double R32, double R33); | ||||
|   Rot3(double w, double x, double y, double z); | ||||
| 
 | ||||
|   static gtsam::Rot3 Rx(double t); | ||||
|   static gtsam::Rot3 Ry(double t); | ||||
|  | @ -980,8 +981,8 @@ class Cal3Bundler { | |||
|   double fy() const; | ||||
|   double k1() const; | ||||
|   double k2() const; | ||||
|   double u0() const; | ||||
|   double v0() const; | ||||
|   double px() const; | ||||
|   double py() const; | ||||
|   Vector vector() const; | ||||
|   Vector k() const; | ||||
|   Matrix K() const; | ||||
|  | @ -2068,7 +2069,7 @@ class NonlinearFactorGraph { | |||
|   gtsam::KeySet keys() const; | ||||
|   gtsam::KeyVector keyVector() const; | ||||
| 
 | ||||
|   template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}> | ||||
|   template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}> | ||||
|   void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); | ||||
| 
 | ||||
|   // NonlinearFactorGraph | ||||
|  | @ -2157,12 +2158,13 @@ class Values { | |||
|   void insert(size_t j, const gtsam::SOn& P); | ||||
|   void insert(size_t j, const gtsam::Rot3& rot3); | ||||
|   void insert(size_t j, const gtsam::Pose3& pose3); | ||||
|   void insert(size_t j, const gtsam::Unit3& unit3); | ||||
|   void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); | ||||
|   void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); | ||||
|   void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); | ||||
|   void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); | ||||
|   void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); | ||||
|   // void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); | ||||
|   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera); | ||||
|   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); | ||||
|   void insert(size_t j, const gtsam::NavState& nav_state); | ||||
| 
 | ||||
|  | @ -2175,18 +2177,19 @@ class Values { | |||
|   void update(size_t j, const gtsam::SOn& P); | ||||
|   void update(size_t j, const gtsam::Rot3& rot3); | ||||
|   void update(size_t j, const gtsam::Pose3& pose3); | ||||
|   void update(size_t j, const gtsam::Unit3& unit3); | ||||
|   void update(size_t j, const gtsam::Cal3_S2& cal3_s2); | ||||
|   void update(size_t j, const gtsam::Cal3DS2& cal3ds2); | ||||
|   void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); | ||||
|   void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); | ||||
|   void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); | ||||
|   // void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); | ||||
|   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera); | ||||
|   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); | ||||
|   void update(size_t j, const gtsam::NavState& nav_state); | ||||
|   void update(size_t j, Vector vector); | ||||
|   void update(size_t j, Matrix matrix); | ||||
| 
 | ||||
|   template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> | ||||
|   template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> | ||||
|   T at(size_t j); | ||||
| 
 | ||||
|   /// version for double | ||||
|  | @ -2490,7 +2493,8 @@ class ISAM2 { | |||
|   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, | ||||
|                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, | ||||
|                      gtsam::Cal3Bundler, gtsam::EssentialMatrix, | ||||
|                      gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}> | ||||
|                      gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,  | ||||
|                      Vector, Matrix}> | ||||
|   VALUE calculateEstimate(size_t key) const; | ||||
|   gtsam::Values calculateBestEstimate() const; | ||||
|   Matrix marginalCovariance(size_t key) const; | ||||
|  | @ -2528,7 +2532,7 @@ class NonlinearISAM { | |||
| #include <gtsam/geometry/StereoPoint2.h> | ||||
| 
 | ||||
| #include <gtsam/nonlinear/PriorFactor.h> | ||||
| template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}> | ||||
| template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}> | ||||
| virtual class PriorFactor : gtsam::NoiseModelFactor { | ||||
|   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); | ||||
|   T prior() const; | ||||
|  | @ -2673,6 +2677,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { | |||
| typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; | ||||
| //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 | ||||
| //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2; | ||||
| typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler; | ||||
| 
 | ||||
| template<CALIBRATION = {gtsam::Cal3_S2}> | ||||
| virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { | ||||
|  | @ -2759,21 +2764,36 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { | |||
| }; | ||||
| 
 | ||||
| #include <gtsam/slam/dataset.h> | ||||
| 
 | ||||
| class SfmTrack { | ||||
|   Point3 point3() const; | ||||
|   SfmTrack(); | ||||
|   SfmTrack(const gtsam::Point3& pt); | ||||
|   const Point3& point3() const; | ||||
| 
 | ||||
|   double r; | ||||
|   double g; | ||||
|   double b; | ||||
|   // TODO Need to close wrap#10 to allow this to work. | ||||
|   // std::vector<pair<size_t, gtsam::Point2>> measurements; | ||||
| 
 | ||||
|   size_t number_measurements() const; | ||||
|   pair<size_t, gtsam::Point2> measurement(size_t idx) const; | ||||
|   pair<size_t, size_t> siftIndex(size_t idx) const; | ||||
|   void add_measurement(size_t idx, const gtsam::Point2& m); | ||||
| }; | ||||
| 
 | ||||
| class SfmData { | ||||
|   SfmData(); | ||||
|   size_t number_cameras() const; | ||||
|   size_t number_tracks() const; | ||||
|   gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const; | ||||
|   gtsam::SfmTrack track(size_t idx) const; | ||||
|   void add_track(const gtsam::SfmTrack& t) ; | ||||
|   void add_camera(const gtsam::SfmCamera& cam); | ||||
| }; | ||||
| 
 | ||||
| gtsam::SfmData readBal(string filename); | ||||
| bool writeBAL(string filename, gtsam::SfmData& data); | ||||
| gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); | ||||
| gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); | ||||
| 
 | ||||
|  |  | |||
|  | @ -164,8 +164,16 @@ inline Key Y(std::uint64_t j) { return Symbol('y', j); } | |||
| inline Key Z(std::uint64_t j) { return Symbol('z', j); } | ||||
| } | ||||
| 
 | ||||
| /** Generates symbol shorthands with alternative names different than the
 | ||||
|  * one-letter predefined ones. */ | ||||
| class SymbolGenerator { | ||||
|   const char c_; | ||||
| public: | ||||
|   SymbolGenerator(const char c) : c_(c) {} | ||||
|   Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); } | ||||
| }; | ||||
| 
 | ||||
| /// traits
 | ||||
| template<> struct traits<Symbol> : public Testable<Symbol> {}; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -40,6 +40,25 @@ TEST(Key, KeySymbolConversion) { | |||
|   EXPECT(assert_equal(original, actual)) | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Key, SymbolGenerator) { | ||||
|   const auto x1 = gtsam::symbol_shorthand::X(1); | ||||
|   const auto v1 = gtsam::symbol_shorthand::V(1); | ||||
|   const auto a1 = gtsam::symbol_shorthand::A(1); | ||||
| 
 | ||||
|   const auto Z = gtsam::SymbolGenerator('x'); | ||||
|   const auto DZ = gtsam::SymbolGenerator('v'); | ||||
|   const auto DDZ = gtsam::SymbolGenerator('a'); | ||||
| 
 | ||||
|   const auto z1 = Z(1); | ||||
|   const auto dz1 = DZ(1); | ||||
|   const auto ddz1 = DDZ(1); | ||||
| 
 | ||||
|   EXPECT(assert_equal(x1, z1)); | ||||
|   EXPECT(assert_equal(v1, dz1)); | ||||
|   EXPECT(assert_equal(a1, ddz1)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| template<int KeySize> | ||||
| Key KeyTestValue(); | ||||
|  | @ -106,4 +125,3 @@ int main() { | |||
|   return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
|  |  | |||
|  | @ -200,6 +200,10 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system, | |||
|     currentValues = system.advance(prevValues, alpha, direction); | ||||
|     currentError = system.error(currentValues); | ||||
| 
 | ||||
|     // User hook:
 | ||||
|     if (params.iterationHook) | ||||
|       params.iterationHook(iteration, prevError, currentError); | ||||
| 
 | ||||
|     // Maybe show output
 | ||||
|     if (params.verbosity >= NonlinearOptimizerParams::ERROR) | ||||
|       std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; | ||||
|  |  | |||
|  | @ -88,20 +88,28 @@ void NonlinearOptimizer::defaultOptimize() { | |||
|   } | ||||
| 
 | ||||
|   // Iterative loop
 | ||||
|   double newError = currentError; // used to avoid repeated calls to error()
 | ||||
|   do { | ||||
|     // Do next iteration
 | ||||
|     currentError = error(); // TODO(frank): don't do this twice at first !? Computed above!
 | ||||
|     currentError = newError; | ||||
|     iterate(); | ||||
|     tictoc_finishedIteration(); | ||||
| 
 | ||||
|     // Update newError for either printouts or conditional-end checks:
 | ||||
|     newError = error(); | ||||
| 
 | ||||
|     // User hook:
 | ||||
|     if (params.iterationHook) | ||||
|       params.iterationHook(iterations(), currentError, newError); | ||||
| 
 | ||||
|     // Maybe show output
 | ||||
|     if (params.verbosity >= NonlinearOptimizerParams::VALUES) | ||||
|       values().print("newValues"); | ||||
|     if (params.verbosity >= NonlinearOptimizerParams::ERROR) | ||||
|       cout << "newError: " << error() << endl; | ||||
|       cout << "newError: " << newError << endl; | ||||
|   } while (iterations() < params.maxIterations && | ||||
|            !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, | ||||
|                              currentError, error(), params.verbosity) && std::isfinite(currentError)); | ||||
|                              currentError, newError, params.verbosity) && std::isfinite(currentError)); | ||||
| 
 | ||||
|   // Printing if verbose
 | ||||
|   if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { | ||||
|  |  | |||
|  | @ -81,7 +81,7 @@ protected: | |||
| 
 | ||||
| public: | ||||
|   /** A shared pointer to this class */ | ||||
|   typedef boost::shared_ptr<const NonlinearOptimizer> shared_ptr; | ||||
|   using shared_ptr = boost::shared_ptr<const NonlinearOptimizer>; | ||||
| 
 | ||||
|   /// @name Standard interface
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -38,21 +38,12 @@ public: | |||
|     SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR | ||||
|   }; | ||||
| 
 | ||||
|   size_t maxIterations; ///< The maximum iterations to stop iterating (default 100)
 | ||||
|   double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5)
 | ||||
|   double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
 | ||||
|   double errorTol; ///< The maximum total error to stop iterating (default 0.0)
 | ||||
|   Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT)
 | ||||
|   Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD)
 | ||||
| 
 | ||||
|   NonlinearOptimizerParams() : | ||||
|       maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( | ||||
|           0.0), verbosity(SILENT), orderingType(Ordering::COLAMD), | ||||
|           linearSolverType(MULTIFRONTAL_CHOLESKY) {} | ||||
| 
 | ||||
|   virtual ~NonlinearOptimizerParams() { | ||||
|   } | ||||
|   virtual void print(const std::string& str = "") const; | ||||
|   size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100)
 | ||||
|   double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5)
 | ||||
|   double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
 | ||||
|   double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0)
 | ||||
|   Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT)
 | ||||
|   Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD)
 | ||||
| 
 | ||||
|   size_t getMaxIterations() const { return maxIterations; } | ||||
|   double getRelativeErrorTol() const { return relativeErrorTol; } | ||||
|  | @ -71,6 +62,37 @@ public: | |||
|   static Verbosity verbosityTranslator(const std::string &s) ; | ||||
|   static std::string verbosityTranslator(Verbosity value) ; | ||||
| 
 | ||||
|   /** Type for an optional user-provided hook to be called after each
 | ||||
|    * internal optimizer iteration. See iterationHook below. */ | ||||
|   using IterationHook = std::function< | ||||
|     void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; | ||||
| 
 | ||||
|   /** Optional user-provided iteration hook to be called after each
 | ||||
|    * optimization iteration (Default: none). | ||||
|    * Note that `IterationHook` is defined as a std::function<> with this | ||||
|    * signature: | ||||
|    * \code | ||||
|    *  void(size_t iteration, double errorBefore, double errorAfter) | ||||
|    * \endcode | ||||
|    * which allows binding by means of a reference to a regular function: | ||||
|    * \code | ||||
|    *  void foo(size_t iteration, double errorBefore, double errorAfter); | ||||
|    *  // ...
 | ||||
|    *  lmOpts.iterationHook = &foo; | ||||
|    * \endcode | ||||
|    * or to a C++11 lambda (preferred if you need to capture additional | ||||
|    * context variables, such that the optimizer object itself, the factor graph, | ||||
|    * etc.): | ||||
|    * \code | ||||
|    *  lmOpts.iterationHook = [&](size_t iter, double oldError, double newError) | ||||
|    *  { | ||||
|    *    // ...
 | ||||
|    *  }; | ||||
|    * \endcode | ||||
|    * or to the result of a properly-formed `std::bind` call. | ||||
|    */ | ||||
|   IterationHook iterationHook; | ||||
| 
 | ||||
|   /** See NonlinearOptimizerParams::linearSolverType */ | ||||
|   enum LinearSolverType { | ||||
|     MULTIFRONTAL_CHOLESKY, | ||||
|  | @ -81,10 +103,16 @@ public: | |||
|     CHOLMOD, /* Experimental Flag */ | ||||
|   }; | ||||
| 
 | ||||
|   LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
 | ||||
|   LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer
 | ||||
|   boost::optional<Ordering> ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty)
 | ||||
|   IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
 | ||||
| 
 | ||||
|   NonlinearOptimizerParams() = default; | ||||
|   virtual ~NonlinearOptimizerParams() { | ||||
|   } | ||||
| 
 | ||||
|   virtual void print(const std::string& str = "") const; | ||||
| 
 | ||||
|   inline bool isMultifrontal() const { | ||||
|     return (linearSolverType == MULTIFRONTAL_CHOLESKY) | ||||
|         || (linearSolverType == MULTIFRONTAL_QR); | ||||
|  |  | |||
|  | @ -41,7 +41,7 @@ struct Cal3Bundler0 : public Cal3Bundler { | |||
|                double v0 = 0) | ||||
|       : Cal3Bundler(f, k1, k2, u0, v0) {} | ||||
|   Cal3Bundler0 retract(const Vector& d) const { | ||||
|     return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); | ||||
|     return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py()); | ||||
|   } | ||||
|   Vector3 localCoordinates(const Cal3Bundler0& T2) const { | ||||
|     return T2.vector() - vector(); | ||||
|  |  | |||
|  | @ -19,6 +19,7 @@ | |||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/FunctorizedFactor.h> | ||||
| #include <gtsam/nonlinear/factorTesting.h> | ||||
|  | @ -115,16 +116,6 @@ TEST(FunctorizedFactor, Print) { | |||
|   auto factor = | ||||
|       MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier)); | ||||
| 
 | ||||
|   // redirect output to buffer so we can compare
 | ||||
|   stringstream buffer; | ||||
|   streambuf *old = cout.rdbuf(buffer.rdbuf()); | ||||
| 
 | ||||
|   factor.print(); | ||||
| 
 | ||||
|   // get output string and reset stdout
 | ||||
|   string actual = buffer.str(); | ||||
|   cout.rdbuf(old); | ||||
| 
 | ||||
|   string expected = | ||||
|       "  keys = { X0 }\n" | ||||
|       "  noise model: unit (9) \n" | ||||
|  | @ -135,7 +126,7 @@ TEST(FunctorizedFactor, Print) { | |||
|       "]\n" | ||||
|       "  noise model sigmas: 1 1 1 1 1 1 1 1 1\n"; | ||||
| 
 | ||||
|   CHECK_EQUAL(expected, actual); | ||||
|   EXPECT(assert_print_equal(expected, factor)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -595,15 +595,7 @@ TEST(Values, Demangle) { | |||
|   values.insert(key1, v); | ||||
|   string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, 3>)\n[\n	5, 6, 7\n]\n\n"; | ||||
| 
 | ||||
|   stringstream buffer; | ||||
|   streambuf * old = cout.rdbuf(buffer.rdbuf()); | ||||
| 
 | ||||
|   values.print(); | ||||
| 
 | ||||
|   string actual = buffer.str(); | ||||
|   cout.rdbuf(old); | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, actual)); | ||||
|   EXPECT(assert_print_equal(expected, values)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -121,8 +121,8 @@ MFAS::MFAS(const TranslationEdges& relativeTranslations, | |||
|   } | ||||
| } | ||||
| 
 | ||||
| vector<Key> MFAS::computeOrdering() const { | ||||
|   vector<Key> ordering;  // Nodes in MFAS order (result).
 | ||||
| KeyVector MFAS::computeOrdering() const { | ||||
|   KeyVector ordering;  // Nodes in MFAS order (result).
 | ||||
| 
 | ||||
|   // A graph is an unordered map from keys to nodes. Each node contains a list
 | ||||
|   // of its adjacent nodes. Create the graph from the edgeWeights.
 | ||||
|  | @ -140,7 +140,7 @@ vector<Key> MFAS::computeOrdering() const { | |||
| 
 | ||||
| map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const { | ||||
|   // Find the ordering.
 | ||||
|   vector<Key> ordering = computeOrdering(); | ||||
|   KeyVector ordering = computeOrdering(); | ||||
| 
 | ||||
|   // Create a map from the node key to its position in the ordering. This makes
 | ||||
|   // it easier to lookup positions of different nodes.
 | ||||
|  |  | |||
|  | @ -84,7 +84,7 @@ class MFAS { | |||
|    * @brief Computes the 1D MFAS ordering of nodes in the graph | ||||
|    * @return orderedNodes: vector of nodes in the obtained order | ||||
|    */ | ||||
|   std::vector<Key> computeOrdering() const; | ||||
|   KeyVector computeOrdering() const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * @brief Computes the outlier weights of the graph. We define the outlier | ||||
|  |  | |||
|  | @ -25,7 +25,7 @@ using namespace gtsam; | |||
| vector<MFAS::KeyPair> edges = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1), | ||||
|                          make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)}; | ||||
| // nodes in the graph
 | ||||
| vector<Key> nodes = {Key(0), Key(1), Key(2), Key(3)}; | ||||
| KeyVector nodes = {Key(0), Key(1), Key(2), Key(3)}; | ||||
| // weights from projecting in direction-1 (bad direction, outlier accepted)
 | ||||
| vector<double> weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75}; | ||||
| // weights from projecting in direction-2 (good direction, outlier rejected)
 | ||||
|  | @ -47,10 +47,10 @@ map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges, | |||
| TEST(MFAS, OrderingWeights2) { | ||||
|   MFAS mfas_obj(getEdgeWeights(edges, weights2)); | ||||
| 
 | ||||
|   vector<Key> ordered_nodes = mfas_obj.computeOrdering(); | ||||
|   KeyVector ordered_nodes = mfas_obj.computeOrdering(); | ||||
| 
 | ||||
|   // ground truth (expected) ordering in this example
 | ||||
|   vector<Key> gt_ordered_nodes = {0, 1, 3, 2}; | ||||
|   KeyVector gt_ordered_nodes = {0, 1, 3, 2}; | ||||
| 
 | ||||
|   // check if the expected ordering is obtained
 | ||||
|   for (size_t i = 0; i < ordered_nodes.size(); i++) { | ||||
|  | @ -77,10 +77,10 @@ TEST(MFAS, OrderingWeights2) { | |||
| TEST(MFAS, OrderingWeights1) { | ||||
|   MFAS mfas_obj(getEdgeWeights(edges, weights1)); | ||||
| 
 | ||||
|   vector<Key> ordered_nodes = mfas_obj.computeOrdering(); | ||||
|   KeyVector ordered_nodes = mfas_obj.computeOrdering(); | ||||
| 
 | ||||
|   // "ground truth" expected ordering in this example
 | ||||
|   vector<Key> gt_ordered_nodes = {3, 0, 1, 2}; | ||||
|   KeyVector gt_ordered_nodes = {3, 0, 1, 2}; | ||||
| 
 | ||||
|   // check if the expected ordering is obtained
 | ||||
|   for (size_t i = 0; i < ordered_nodes.size(); i++) { | ||||
|  |  | |||
|  | @ -1164,8 +1164,8 @@ bool writeBAL(const string &filename, SfmData &data) { | |||
|     for (size_t k = 0; k < track.number_measurements(); | ||||
|          k++) { // for each observation of the 3D point j
 | ||||
|       size_t i = track.measurements[k].first; // camera id
 | ||||
|       double u0 = data.cameras[i].calibration().u0(); | ||||
|       double v0 = data.cameras[i].calibration().v0(); | ||||
|       double u0 = data.cameras[i].calibration().px(); | ||||
|       double v0 = data.cameras[i].calibration().py(); | ||||
| 
 | ||||
|       if (u0 != 0 || v0 != 0) { | ||||
|         cout << "writeBAL has not been tested for calibration with nonzero " | ||||
|  |  | |||
|  | @ -211,16 +211,18 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); | |||
| /// A measurement with its camera index
 | ||||
| typedef std::pair<size_t, Point2> SfmMeasurement; | ||||
| 
 | ||||
| /// SfmTrack
 | ||||
| /// Sift index for SfmTrack
 | ||||
| typedef std::pair<size_t, size_t> SiftIndex; | ||||
| 
 | ||||
| /// Define the structure for the 3D points
 | ||||
| struct SfmTrack { | ||||
|   SfmTrack(): p(0,0,0) {} | ||||
|   SfmTrack(const gtsam::Point3& pt) : p(pt) {} | ||||
|   Point3 p; ///< 3D position of the point
 | ||||
|   float r, g, b; ///< RGB color of the 3D point
 | ||||
|   std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
 | ||||
|   std::vector<SiftIndex> siftIndices; | ||||
|    | ||||
|   /// Total number of measurements in this track
 | ||||
|   size_t number_measurements() const { | ||||
|     return measurements.size(); | ||||
|  | @ -233,11 +235,17 @@ struct SfmTrack { | |||
|   SiftIndex siftIndex(size_t idx) const { | ||||
|     return siftIndices[idx]; | ||||
|   } | ||||
|   Point3 point3() const { | ||||
|   /// Get 3D point
 | ||||
|   const Point3& point3() const { | ||||
|     return p; | ||||
|   } | ||||
|   /// Add measurement (camera_idx, Point2) to track
 | ||||
|   void add_measurement(size_t idx, const gtsam::Point2& m) { | ||||
|     measurements.emplace_back(idx, m); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| /// Define the structure for the camera poses
 | ||||
| typedef PinholeCamera<Cal3Bundler> SfmCamera; | ||||
| 
 | ||||
|  | @ -260,6 +268,14 @@ struct SfmData { | |||
|   SfmTrack track(size_t idx) const { | ||||
|     return tracks[idx]; | ||||
|   } | ||||
|   /// Add a track to SfmData
 | ||||
|   void add_track(const SfmTrack& t)  { | ||||
|     tracks.push_back(t); | ||||
|   } | ||||
|   /// Add a camera to SfmData
 | ||||
|   void add_camera(const SfmCamera& cam){ | ||||
|     cameras.push_back(cam); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  |  | |||
|  | @ -361,8 +361,12 @@ TEST(Similarity3, AlignPose3) { | |||
| 
 | ||||
|   vector<Pose3Pair> correspondences{bTa1, bTa2}; | ||||
| 
 | ||||
|   // Cayley transform cannot accommodate 180 degree rotations,
 | ||||
|   // hence we only test for Expmap
 | ||||
| #ifdef GTSAM_ROT3_EXPMAP | ||||
|   Similarity3 actual_aSb = Similarity3::Align(correspondences); | ||||
|   EXPECT(assert_equal(expected_aSb, actual_aSb)); | ||||
| #endif | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  |  | |||
|  | @ -39,7 +39,7 @@ | |||
| | SelfCalibrationExample                                |        | | ||||
| | SFMdata                                               |        |      | ||||
| | SFMExample_bal_COLAMD_METIS                           |        | | ||||
| | SFMExample_bal                                        |       | | ||||
| | SFMExample_bal                                        | :heavy_check_mark:      | | ||||
| | SFMExample                                            | :heavy_check_mark:      | | ||||
| | SFMExampleExpressions_bal                             |        | | ||||
| | SFMExampleExpressions                                 |        | | ||||
|  |  | |||
|  | @ -0,0 +1,118 @@ | |||
| """ | ||||
|   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 | ||||
| 
 | ||||
|   Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file | ||||
|   Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert) | ||||
| """ | ||||
| 
 | ||||
| import argparse | ||||
| import logging | ||||
| import sys | ||||
| 
 | ||||
| import matplotlib.pyplot as plt | ||||
| import numpy as np | ||||
| 
 | ||||
| import gtsam | ||||
| from gtsam import ( | ||||
|     GeneralSFMFactorCal3Bundler, | ||||
|     PinholeCameraCal3Bundler, | ||||
|     PriorFactorPinholeCameraCal3Bundler, | ||||
|     readBal, | ||||
|     symbol_shorthand | ||||
| ) | ||||
| 
 | ||||
| C = symbol_shorthand.C | ||||
| P = symbol_shorthand.P | ||||
| 
 | ||||
| 
 | ||||
| logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) | ||||
| 
 | ||||
| def run(args): | ||||
|     """ Run LM optimization with BAL input data and report resulting error """ | ||||
|     input_file = gtsam.findExampleDataFile(args.input_file) | ||||
| 
 | ||||
|     # Load the SfM data from file | ||||
|     scene_data = readBal(input_file) | ||||
|     logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n") | ||||
| 
 | ||||
|     # Create a factor graph | ||||
|     graph = gtsam.NonlinearFactorGraph() | ||||
| 
 | ||||
|     # We share *one* noiseModel between all projection factors | ||||
|     noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v | ||||
| 
 | ||||
|     # Add measurements to the factor graph | ||||
|     j = 0 | ||||
|     for t_idx in range(scene_data.number_tracks()): | ||||
|         track = scene_data.track(t_idx) # SfmTrack | ||||
|         # retrieve the SfmMeasurement objects | ||||
|         for m_idx in range(track.number_measurements()): | ||||
|             # i represents the camera index, and uv is the 2d measurement | ||||
|             i, uv = track.measurement(m_idx) | ||||
|             # note use of shorthand symbols C and P | ||||
|             graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) | ||||
|         j += 1 | ||||
| 
 | ||||
|     # Add a prior on pose x1. This indirectly specifies where the origin is. | ||||
|     graph.push_back( | ||||
|         gtsam.PriorFactorPinholeCameraCal3Bundler( | ||||
|             C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) | ||||
|         ) | ||||
|     ) | ||||
|     # Also add a prior on the position of the first landmark to fix the scale | ||||
|     graph.push_back( | ||||
|         gtsam.PriorFactorPoint3( | ||||
|             P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) | ||||
|         ) | ||||
|     ) | ||||
| 
 | ||||
|     # Create initial estimate | ||||
|     initial = gtsam.Values() | ||||
|      | ||||
|     i = 0 | ||||
|     # add each PinholeCameraCal3Bundler | ||||
|     for cam_idx in range(scene_data.number_cameras()): | ||||
|         camera = scene_data.camera(cam_idx) | ||||
|         initial.insert(C(i), camera) | ||||
|         i += 1 | ||||
| 
 | ||||
|     j = 0 | ||||
|     # add each SfmTrack | ||||
|     for t_idx in range(scene_data.number_tracks()): | ||||
|         track = scene_data.track(t_idx)   | ||||
|         initial.insert(P(j), track.point3()) | ||||
|         j += 1 | ||||
| 
 | ||||
|     # Optimize the graph and print results | ||||
|     try: | ||||
|         params = gtsam.LevenbergMarquardtParams() | ||||
|         params.setVerbosityLM("ERROR") | ||||
|         lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) | ||||
|         result = lm.optimize() | ||||
|     except Exception as e: | ||||
|         logging.exception("LM Optimization failed") | ||||
|         return | ||||
|     # Error drops from ~2764.22 to ~0.046 | ||||
|     logging.info(f"final error: {graph.error(result)}") | ||||
| 
 | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     parser = argparse.ArgumentParser() | ||||
|     parser.add_argument( | ||||
|         '-i', | ||||
|         '--input_file', | ||||
|         type=str, | ||||
|         default="dubrovnik-3-7-pre", | ||||
|         help='Read SFM data from the specified BAL file' | ||||
|         'The data format is described here: https://grail.cs.washington.edu/projects/bal/.' | ||||
|         'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, ' | ||||
|         'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector' | ||||
|         'and (x,y,z) 3d point initializations.' | ||||
|     ) | ||||
|     run(parser.parse_args()) | ||||
| 
 | ||||
|  | @ -0,0 +1,79 @@ | |||
| """ | ||||
| GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, | ||||
| Atlanta, Georgia 30332-0415 | ||||
| All Rights Reserved | ||||
| 
 | ||||
| See LICENSE for the license information | ||||
| 
 | ||||
| Unit tests for testing dataset access. | ||||
| Author: Frank Dellaert (Python: Sushmita Warrier) | ||||
| """ | ||||
| # pylint: disable=invalid-name, no-name-in-module, no-member | ||||
| 
 | ||||
| from __future__ import print_function | ||||
| 
 | ||||
| import unittest | ||||
| 
 | ||||
| import numpy as np | ||||
| 
 | ||||
| import gtsam | ||||
| from gtsam.utils.test_case import GtsamTestCase | ||||
| 
 | ||||
| 
 | ||||
| class TestSfmData(GtsamTestCase): | ||||
|     """Tests for SfmData and SfmTrack modules.""" | ||||
| 
 | ||||
|     def setUp(self): | ||||
|         """Initialize SfmData and SfmTrack""" | ||||
|         self.data = gtsam.SfmData() | ||||
|         # initialize SfmTrack with 3D point | ||||
|         self.tracks = gtsam.SfmTrack() | ||||
| 
 | ||||
|     def test_tracks(self): | ||||
|         """Test functions in SfmTrack""" | ||||
|         # measurement is of format (camera_idx, imgPoint) | ||||
|         # create arbitrary camera indices for two cameras | ||||
|         i1, i2 = 4,5 | ||||
|         # create arbitrary image measurements for cameras i1 and i2 | ||||
|         uv_i1 = gtsam.Point2(12.6, 82) | ||||
|         # translating point uv_i1 along X-axis | ||||
|         uv_i2 = gtsam.Point2(24.88, 82) | ||||
|         # add measurements to the track | ||||
|         self.tracks.add_measurement(i1, uv_i1) | ||||
|         self.tracks.add_measurement(i2, uv_i2) | ||||
|         # Number of measurements in the track is 2 | ||||
|         self.assertEqual(self.tracks.number_measurements(), 2) | ||||
|         # camera_idx in the first measurement of the track corresponds to i1 | ||||
|         cam_idx, img_measurement = self.tracks.measurement(0) | ||||
|         self.assertEqual(cam_idx, i1) | ||||
|         np.testing.assert_array_almost_equal( | ||||
|             gtsam.Point3(0.,0.,0.),  | ||||
|             self.tracks.point3() | ||||
|         ) | ||||
| 
 | ||||
| 
 | ||||
|     def test_data(self): | ||||
|         """Test functions in SfmData""" | ||||
|         # Create new track with 3 measurements | ||||
|         i1, i2, i3 = 3,5,6 | ||||
|         uv_i1 = gtsam.Point2(21.23, 45.64) | ||||
|         # translating along X-axis | ||||
|         uv_i2 = gtsam.Point2(45.7, 45.64) | ||||
|         uv_i3 = gtsam.Point2(68.35, 45.64) | ||||
|         # add measurements and arbitrary point to the track | ||||
|         measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] | ||||
|         pt = gtsam.Point3(1.0, 6.0, 2.0) | ||||
|         track2 = gtsam.SfmTrack(pt) | ||||
|         track2.add_measurement(i1, uv_i1) | ||||
|         track2.add_measurement(i2, uv_i2) | ||||
|         track2.add_measurement(i3, uv_i3) | ||||
|         self.data.add_track(self.tracks) | ||||
|         self.data.add_track(track2) | ||||
|         # Number of tracks in SfmData is 2 | ||||
|         self.assertEqual(self.data.number_tracks(), 2) | ||||
|         # camera idx of first measurement of second track corresponds to i1 | ||||
|         cam_idx, img_measurement = self.data.track(1).measurement(0) | ||||
|         self.assertEqual(cam_idx, i1) | ||||
| 
 | ||||
| if __name__ == '__main__': | ||||
|     unittest.main() | ||||
|  | @ -566,6 +566,58 @@ TEST( NonlinearOptimizer, logfile ) | |||
| //  EXPECT(actual.str()==expected.str());
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( NonlinearOptimizer, iterationHook_LM ) | ||||
| { | ||||
|   NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); | ||||
| 
 | ||||
|   Point2 x0(3,3); | ||||
|   Values c0; | ||||
|   c0.insert(X(1), x0); | ||||
| 
 | ||||
|   // Levenberg-Marquardt
 | ||||
|   LevenbergMarquardtParams lmParams; | ||||
|   size_t lastIterCalled = 0; | ||||
|   lmParams.iterationHook = [&](size_t iteration, double oldError, double newError) | ||||
|   { | ||||
|     // Tests:
 | ||||
|     lastIterCalled = iteration; | ||||
|     EXPECT(newError<oldError); | ||||
|      | ||||
|     // Example of evolution printout:
 | ||||
|     //std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
 | ||||
|   }; | ||||
|   LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); | ||||
|    | ||||
|   EXPECT(lastIterCalled>5); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| TEST( NonlinearOptimizer, iterationHook_CG ) | ||||
| { | ||||
|   NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); | ||||
| 
 | ||||
|   Point2 x0(3,3); | ||||
|   Values c0; | ||||
|   c0.insert(X(1), x0); | ||||
| 
 | ||||
|   // Levenberg-Marquardt
 | ||||
|   NonlinearConjugateGradientOptimizer::Parameters cgParams; | ||||
|   size_t lastIterCalled = 0; | ||||
|   cgParams.iterationHook = [&](size_t iteration, double oldError, double newError) | ||||
|   { | ||||
|     // Tests:
 | ||||
|     lastIterCalled = iteration; | ||||
|     EXPECT(newError<oldError); | ||||
|      | ||||
|     // Example of evolution printout:
 | ||||
|     //std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
 | ||||
|   }; | ||||
|   NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize(); | ||||
|    | ||||
|   EXPECT(lastIterCalled>5); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //// Minimal traits example
 | ||||
| struct MyType : public Vector3 { | ||||
|  |  | |||
|  | @ -146,7 +146,7 @@ function(install_python_scripts | |||
|       else() | ||||
|         set(build_type_tag "") | ||||
|       endif() | ||||
|       # Split up filename to strip trailing '/' in WRAP_CYTHON_INSTALL_PATH if | ||||
|       # Split up filename to strip trailing '/' in GTSAM_PY_INSTALL_PATH if | ||||
|       # there is one | ||||
|       get_filename_component(location "${dest_directory}" PATH) | ||||
|       get_filename_component(name "${dest_directory}" NAME) | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue