diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index d890577b6..b5a559df5 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -71,6 +71,7 @@ function configure() -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \ -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ + -DGTSAM_SINGLE_TEST_EXE=ON \ -DBOOST_ROOT=$BOOST_ROOT \ -DBoost_NO_SYSTEM_PATHS=ON \ -DBoost_ARCHITECTURE=-x64 @@ -95,7 +96,11 @@ function build () configure if [ "$(uname)" == "Linux" ]; then - make -j$(nproc) + if (($(nproc) > 2)); then + make -j$(nproc) + else + make -j2 + fi elif [ "$(uname)" == "Darwin" ]; then make -j$(sysctl -n hw.physicalcpu) fi @@ -113,7 +118,11 @@ function test () # Actual testing if [ "$(uname)" == "Linux" ]; then - make -j$(nproc) check + if (($(nproc) > 2)); then + make -j$(nproc) check + else + make -j2 check + fi elif [ "$(uname)" == "Darwin" ]; then make -j$(sysctl -n hw.physicalcpu) check fi diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index a9e794b3f..ef2500b46 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -48,7 +48,9 @@ jobs: - name: Install Dependencies shell: powershell run: | - Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh') + iwr -useb get.scoop.sh -outfile 'install_scoop.ps1' + .\install_scoop.ps1 -RunAsAdmin + scoop install cmake --global # So we don't get issues with CMP0074 policy scoop install ninja --global @@ -106,6 +108,21 @@ jobs: cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap + + # Run GTSAM tests cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.basis + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.discrete + #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.inference cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.navigation + #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear + #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sfm + #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.symbolic + + # Run GTSAM_UNSTABLE tests + #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable + diff --git a/CMakeLists.txt b/CMakeLists.txt index c637796cf..cfb251663 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,3 @@ -project(GTSAM CXX C) cmake_minimum_required(VERSION 3.0) # new feature to Cmake Version > 2.8.12 @@ -11,7 +10,7 @@ endif() set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 2) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "a5") +set (GTSAM_PRERELEASE_VERSION "a6") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") if (${GTSAM_VERSION_PATCH} EQUAL 0) @@ -19,6 +18,11 @@ if (${GTSAM_VERSION_PATCH} EQUAL 0) else() set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}${GTSAM_PRERELEASE_VERSION}") endif() + +project(GTSAM + LANGUAGES CXX C + VERSION "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") + message(STATUS "GTSAM Version: ${GTSAM_VERSION_STRING}") set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) diff --git a/DEVELOP.md b/DEVELOP.md index 8604afe0f..7cd303373 100644 --- a/DEVELOP.md +++ b/DEVELOP.md @@ -15,7 +15,7 @@ For example: ```cpp class GTSAM_EXPORT MyClass { ... }; -GTSAM_EXPORT myFunction(); +GTSAM_EXPORT return_type myFunction(); ``` More details [here](Using-GTSAM-EXPORT.md). diff --git a/INSTALL.md b/INSTALL.md index 965246304..1edccd3cd 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -13,7 +13,7 @@ $ make install ## Important Installation Notes 1. GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes). + - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes) for version recommendations based on your compiler. - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher @@ -72,7 +72,7 @@ execute commands as follows for an out-of-source build: Versions of Boost prior to 1.65 have a known bug that prevents proper "deep" serialization of objects, which means that objects encapsulated inside other objects don't get serialized. This is particularly seen when using `clang` as the C++ compiler. -For this reason we require Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager. +For this reason we recommend Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager. ## Known Issues diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md index faeebc97f..24a29f96b 100644 --- a/Using-GTSAM-EXPORT.md +++ b/Using-GTSAM-EXPORT.md @@ -8,6 +8,7 @@ To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and need * At least one of the functions inside that class is declared in a .cpp file and not just the .h file. * You can `GTSAM_EXPORT` any class it inherits from as well. (Note that this implictly requires the class does not derive from a "header-only" class. Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!) 3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.) +4. For template specializations, you need to add `GTSAM_EXPORT` to each individual specialization. ## When is GTSAM_EXPORT being used incorrectly Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in `gtsam/base` will often show up when compiling the `check_base_program` or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like: diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 4b179d128..9058807ad 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -93,6 +93,10 @@ if(MSVC) /wd4267 # warning C4267: 'initializing': conversion from 'size_t' to 'int', possible loss of data ) + add_compile_options(/wd4005) + add_compile_options(/wd4101) + add_compile_options(/wd4834) + endif() # Other (non-preprocessor macros) compiler flags: @@ -187,7 +191,7 @@ endif() if (NOT MSVC) option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON) - if(GTSAM_BUILD_WITH_MARCH_NATIVE) + if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64")) # Add as public flag so all dependant projects also use it, as required # by Eigen to avid crashes due to SIMD vectorization: list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native") diff --git a/cmake/HandleMetis.cmake b/cmake/HandleMetis.cmake index 9c29e5776..5cbec4ff5 100644 --- a/cmake/HandleMetis.cmake +++ b/cmake/HandleMetis.cmake @@ -21,7 +21,12 @@ if(GTSAM_USE_SYSTEM_METIS) mark_as_advanced(METIS_LIBRARY) add_library(metis-gtsam-if INTERFACE) - target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR}) + target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR} + # gtsam_unstable/partition/FindSeparator-inl.h uses internal metislib.h API + # via extern "C" + $ + $ + ) target_link_libraries(metis-gtsam-if INTERFACE ${METIS_LIBRARY}) endif() else() @@ -30,10 +35,12 @@ else() add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis) target_include_directories(metis-gtsam BEFORE PUBLIC + $ $ + # gtsam_unstable/partition/FindSeparator-inl.h uses internal metislib.h API + # via extern "C" $ $ - $ ) add_library(metis-gtsam-if INTERFACE) diff --git a/doc/Code/LocalizationExample2.cpp b/doc/Code/LocalizationExample2.cpp index d22180314..df9469a64 100644 --- a/doc/Code/LocalizationExample2.cpp +++ b/doc/Code/LocalizationExample2.cpp @@ -1,7 +1,7 @@ // add unary measurement factors, like GPS, on all three poses -noiseModel::Diagonal::shared_ptr unaryNoise = +auto unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y -graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); -graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); -graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); +graph.emplace_shared(1, 0.0, 0.0, unaryNoise); +graph.emplace_shared(2, 2.0, 0.0, unaryNoise); +graph.emplace_shared(3, 4.0, 0.0, unaryNoise); diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index d298091dc..2c1f01c43 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -1,13 +1,12 @@ class UnaryFactor: public NoiseModelFactor1 { double mx_, my_; ///< X and Y measurements - + public: UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): NoiseModelFactor1(model, j), mx_(x), my_(y) {} - Vector evaluateError(const Pose2& q, - boost::optional H = boost::none) const - { + Vector evaluateError(const Pose2& q, + boost::optional H = boost::none) const override { const Rot2& R = q.rotation(); if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp index 2befa9dc2..7af27c60c 100644 --- a/doc/Code/OdometryExample.cpp +++ b/doc/Code/OdometryExample.cpp @@ -3,13 +3,11 @@ NonlinearFactorGraph graph; // Add a Gaussian prior on pose x_1 Pose2 priorMean(0.0, 0.0, 0.0); -noiseModel::Diagonal::shared_ptr priorNoise = - noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); -graph.addPrior(1, priorMean, priorNoise); +auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); +graph.add(PriorFactor(1, priorMean, priorNoise)); // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); -noiseModel::Diagonal::shared_ptr odometryNoise = - noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); +auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/doc/Code/OdometryOutput1.txt b/doc/Code/OdometryOutput1.txt index cc34e8ef2..70aba38ee 100644 --- a/doc/Code/OdometryOutput1.txt +++ b/doc/Code/OdometryOutput1.txt @@ -1,11 +1,14 @@ Factor Graph: size: 3 -factor 0: PriorFactor on 1 - prior mean: (0, 0, 0) + +Factor 0: PriorFactor on 1 + prior mean: (0, 0, 0) noise model: diagonal sigmas [0.3; 0.3; 0.1]; -factor 1: BetweenFactor(1,2) - measured: (2, 0, 0) - noise model: diagonal sigmas [0.2; 0.2; 0.1]; -factor 2: BetweenFactor(2,3) - measured: (2, 0, 0) + +Factor 1: BetweenFactor(1,2) + measured: (2, 0, 0) noise model: diagonal sigmas [0.2; 0.2; 0.1]; + +Factor 2: BetweenFactor(2,3) + measured: (2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; \ No newline at end of file diff --git a/doc/Code/OdometryOutput2.txt b/doc/Code/OdometryOutput2.txt index acfa0b95d..6567bea6c 100644 --- a/doc/Code/OdometryOutput2.txt +++ b/doc/Code/OdometryOutput2.txt @@ -1,11 +1,23 @@ Initial Estimate: + Values with 3 values: -Value 1: (0.5, 0, 0.2) -Value 2: (2.3, 0.1, -0.2) -Value 3: (4.1, 0.1, 0.1) +Value 1: (gtsam::Pose2) +(0.5, 0, 0.2) + +Value 2: (gtsam::Pose2) +(2.3, 0.1, -0.2) + +Value 3: (gtsam::Pose2) +(4.1, 0.1, 0.1) Final Result: + Values with 3 values: -Value 1: (-1.8e-16, 8.7e-18, -9.1e-19) -Value 2: (2, 7.4e-18, -2.5e-18) -Value 3: (4, -1.8e-18, -3.1e-18) +Value 1: (gtsam::Pose2) +(7.5-16, -5.3-16, -1.8-16) + +Value 2: (gtsam::Pose2) +(2, -1.1-15, -2.5-16) + +Value 3: (gtsam::Pose2) +(4, -1.7-15, -2.5-16) diff --git a/doc/Code/OdometryOutput3.txt b/doc/Code/OdometryOutput3.txt index e346ccb4d..514d804cd 100644 --- a/doc/Code/OdometryOutput3.txt +++ b/doc/Code/OdometryOutput3.txt @@ -1,12 +1,12 @@ x1 covariance: - 0.09 1.1e-47 5.7e-33 - 1.1e-47 0.09 1.9e-17 - 5.7e-33 1.9e-17 0.01 + 0.09 1.7e-33 2.8e-33 +1.7e-33 0.09 2.6e-17 +2.8e-33 2.6e-17 0.01 x2 covariance: - 0.13 4.7e-18 2.4e-18 - 4.7e-18 0.17 0.02 - 2.4e-18 0.02 0.02 + 0.13 1.2e-18 6.1e-19 +1.2e-18 0.17 0.02 +6.1e-19 0.02 0.02 x3 covariance: - 0.17 2.7e-17 8.4e-18 - 2.7e-17 0.37 0.06 - 8.4e-18 0.06 0.03 + 0.17 8.6e-18 2.7e-18 +8.6e-18 0.37 0.06 +2.7e-18 0.06 0.03 \ No newline at end of file diff --git a/doc/gtsam.lyx b/doc/gtsam.lyx index 29d03cd35..705a84911 100644 --- a/doc/gtsam.lyx +++ b/doc/gtsam.lyx @@ -134,14 +134,10 @@ A Hands-on Introduction \begin_layout Author Frank Dellaert -\begin_inset Newline newline -\end_inset - -Technical Report number GT-RIM-CP&R-2014-XXX \end_layout \begin_layout Date -September 2014 +Updated Last March 2022 \end_layout \begin_layout Standard @@ -154,18 +150,14 @@ filename "common_macros.tex" \end_layout -\begin_layout Section* -Overview -\end_layout - -\begin_layout Standard +\begin_layout Abstract In this document I provide a hands-on introduction to both factor graphs and GTSAM. This is an updated version from the 2012 TR that is tailored to our GTSAM - 3.0 library and beyond. + 4.0 library and beyond. \end_layout -\begin_layout Standard +\begin_layout Abstract \series bold Factor graphs @@ -206,7 +198,7 @@ ts or prior knowledge. robotics and vision. \end_layout -\begin_layout Standard +\begin_layout Abstract The GTSAM toolbox (GTSAM stands for \begin_inset Quotes eld \end_inset @@ -221,11 +213,13 @@ Georgia Tech Smoothing and Mapping It provides state of the art solutions to the SLAM and SFM problems, but can also be used to model and solve both simpler and more complex estimation problems. - It also provides a MATLAB interface which allows for rapid prototype developmen -t, visualization, and user interaction. + It also provides MATLAB and Python wrappers which allow for rapid prototype + development, visualization, and user interaction. + In addition, it is easy to use in Jupyter notebooks and/or Google's coLaborator +y. \end_layout -\begin_layout Standard +\begin_layout Abstract GTSAM exploits sparsity to be computationally efficient. Typically measurements only provide information on the relationship between a handful of variables, and hence the resulting factor graph will be sparsely @@ -236,14 +230,17 @@ l complexity. GTSAM provides iterative methods that are quite efficient regardless. \end_layout -\begin_layout Standard -You can download the latest version of GTSAM at +\begin_layout Abstract +You can download the latest version of GTSAM from GitHub at +\end_layout + +\begin_layout Abstract \begin_inset Flex URL status open \begin_layout Plain Layout -http://tinyurl.com/gtsam +https://github.com/borglab/gtsam \end_layout \end_inset @@ -741,7 +738,7 @@ noindent \begin_inset Formula $f_{0}(x_{1})$ \end_inset - on lines 5-8 as an instance of + on lines 5-7 as an instance of \series bold \emph on PriorFactor @@ -773,7 +770,7 @@ Pose2, noiseModel::Diagonal \series default \emph default - by specifying three standard deviations in line 7, respectively 30 cm. + by specifying three standard deviations in line 6, respectively 30 cm. \begin_inset space ~ \end_inset @@ -795,7 +792,7 @@ Similarly, odometry measurements are specified as Pose2 \series default \emph default - on line 11, with a slightly different noise model defined on line 12-13. + on line 10, with a slightly different noise model defined on line 11. We then add the two factors \begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$ \end_inset @@ -804,7 +801,7 @@ Pose2 \begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$ \end_inset - on lines 14-15, as instances of yet another templated class, + on lines 12-13, as instances of yet another templated class, \series bold \emph on BetweenFactor @@ -875,7 +872,7 @@ smoothing and mapping . Later in this document we will talk about how we can also use GTSAM to - do filtering (which you often do + do filtering (which often you do \emph on not \emph default @@ -928,7 +925,11 @@ Values \begin_layout Standard The latter point is often a point of confusion with beginning users of GTSAM. It helps to remember that when designing GTSAM we took a functional approach - of classes corresponding to mathematical objects, which are usually immutable. + of classes corresponding to mathematical objects, which are usually +\emph on +immutable +\emph default +. You should think of a factor graph as a \emph on function @@ -1027,7 +1028,7 @@ NonlinearFactorGraph \end_layout \begin_layout Standard -The relevant output from running the example is as follows: +The relevant output from running the example is as follows: \family typewriter \size small @@ -1363,14 +1364,18 @@ where \end_inset is the measurement, -\begin_inset Formula $q$ +\begin_inset Formula $q\in SE(2)$ \end_inset is the unknown variable, \begin_inset Formula $h(q)$ \end_inset - is a (possibly nonlinear) measurement function, and + is a +\series bold +measurement function +\series default +, and \begin_inset Formula $\Sigma$ \end_inset @@ -1546,7 +1551,7 @@ E(q)\define h(q)-m \end_inset -which is done on line 12. +which is done on line 14. Importantly, because we want to use this factor for nonlinear optimization (see e.g., \begin_inset CommandInset citation @@ -1599,11 +1604,11 @@ q_{y} \begin_inset Formula $q=\left(q_{x},q_{y},q_{\theta}\right)$ \end_inset -, yields the following simple +, yields the following \begin_inset Formula $2\times3$ \end_inset - matrix in tangent space which is the same the as the rotation matrix: + matrix: \end_layout \begin_layout Standard @@ -1618,6 +1623,171 @@ H=\left[\begin{array}{ccc} \end_inset +\end_layout + +\begin_layout Paragraph* +Important Note +\end_layout + +\begin_layout Standard +Many of our users, when attempting to create a custom factor, are initially + surprised at the Jacobian matrix not agreeing with their intuition. + For example, above you might simply expect a +\begin_inset Formula $2\times3$ +\end_inset + + identity matrix. + This +\emph on +would +\emph default + be true for variables belonging to a vector space. + However, in GTSAM we define the Jacobian more generally to be the matrix + +\begin_inset Formula $H$ +\end_inset + + such that +\begin_inset Formula +\[ +h(q\exp\hat{\xi})\approx h(q)+H\xi +\] + +\end_inset + +where +\begin_inset Formula $\xi=(\delta x,\delta y,\delta\theta)$ +\end_inset + + is an incremental update and +\begin_inset Formula $\exp\hat{\xi}$ +\end_inset + + is the +\series bold +exponential map +\series default + for the variable we want to update. + In this case +\begin_inset Formula $q\in SE(2)$ +\end_inset + +, where +\begin_inset Formula $SE(2)$ +\end_inset + + is the group of 2D rigid transforms, implemented by +\series bold +\emph on +Pose2 +\emph default +. + +\series default +The exponential map for +\begin_inset Formula $SE(2)$ +\end_inset + + can be approximated to first order as +\begin_inset Formula +\[ +\exp\hat{\xi}\approx\left[\begin{array}{ccc} +1 & -\delta\theta & \delta x\\ +\delta\theta & 1 & \delta y\\ +0 & 0 & 1 +\end{array}\right] +\] + +\end_inset + +when using the +\begin_inset Formula $3\times3$ +\end_inset + + matrix representation for 2D poses, and hence +\begin_inset Formula +\[ +h(qe^{\hat{\xi}})\approx h\left(\left[\begin{array}{ccc} +\cos(q_{\theta}) & -\sin(q_{\theta}) & q_{x}\\ +\sin(q_{\theta}) & \cos(q_{\theta}) & q_{y}\\ +0 & 0 & 1 +\end{array}\right]\left[\begin{array}{ccc} +1 & -\delta\theta & \delta x\\ +\delta\theta & 1 & \delta y\\ +0 & 0 & 1 +\end{array}\right]\right)=\left[\begin{array}{c} +q_{x}+\cos(q_{\theta})\delta x-\sin(q_{\theta})\delta y\\ +q_{y}+\sin(q_{\theta})\delta x+\cos(q_{\theta})\delta y +\end{array}\right] +\] + +\end_inset + +which then explains the Jacobian +\begin_inset Formula $H$ +\end_inset + +. +\end_layout + +\begin_layout Standard +Lie groups are very relevant in the robotics context, and you can read more + here: +\end_layout + +\begin_layout Itemize +\begin_inset Flex URL +status open + +\begin_layout Plain Layout + +https://github.com/borglab/gtsam/blob/develop/doc/LieGroups.pdf +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Itemize +\begin_inset Flex URL +status open + +\begin_layout Plain Layout + +https://github.com/borglab/gtsam/blob/develop/doc/math.pdf +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +In some cases you want to go even beyond Lie groups to a looser concept, + +\series bold +manifolds +\series default +, because not all unknown variables behave like a group, e.g., the space of + 3D planes, 2D lines, directions in space, etc. + For manifolds we do not always have an exponential map, but we have a retractio +n that plays the same role. + Some of this is explained here: +\end_layout + +\begin_layout Itemize +\begin_inset Flex URL +status open + +\begin_layout Plain Layout + +https://gtsam.org/notes/GTSAM-Concepts.html +\end_layout + +\end_inset + + \end_layout \begin_layout Subsection @@ -1680,13 +1850,13 @@ UnaryFactor \series default \emph default instances, and add them to graph. - GTSAM uses shared pointers to refer to factors in factor graphs, and + GTSAM uses shared pointers to refer to factors, and \series bold \emph on -boost::make_shared +emplace_shared \series default \emph default - is a convenience function to simultaneously construct a class and create + is a convenience method to simultaneously construct a class and create a \series bold \emph on @@ -1694,22 +1864,6 @@ shared_ptr \series default \emph default to it. - -\begin_inset Note Note -status collapsed - -\begin_layout Plain Layout -and on lines 4-6 we add three newly created -\series bold -\emph on -UnaryFactor -\series default -\emph default - instances to the graph. -\end_layout - -\end_inset - We obtain the factor graph from Figure \begin_inset CommandInset ref LatexCommand vref diff --git a/doc/gtsam.pdf b/doc/gtsam.pdf index d4cb8908f..961c808d0 100644 Binary files a/doc/gtsam.pdf and b/doc/gtsam.pdf differ diff --git a/examples/Data/randomGrid3D.xml b/examples/Data/randomGrid3D.xml index 6a82ce31c..42eb473be 100644 --- a/examples/Data/randomGrid3D.xml +++ b/examples/Data/randomGrid3D.xml @@ -7,7 +7,7 @@ 32 1 - + diff --git a/examples/Data/toy3D.xml b/examples/Data/toy3D.xml index 13dbcbe6c..26bd231ca 100644 --- a/examples/Data/toy3D.xml +++ b/examples/Data/toy3D.xml @@ -7,7 +7,7 @@ 2 1 - + diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index c8e31e1c5..aa61ffc6c 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -10,62 +10,81 @@ * -------------------------------------------------------------------------- */ /** - * @file RangeISAMExample_plaza1.cpp + * @file RangeISAMExample_plaza2.cpp * @brief A 2D Range SLAM example * @date June 20, 2013 - * @author FRank Dellaert + * @author Frank Dellaert */ -// Both relative poses and recovered trajectory poses will be stored as Pose2 objects +// Both relative poses and recovered trajectory poses will be stored as Pose2. #include +using gtsam::Pose2; -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols +// gtsam::Vectors are dynamic Eigen vectors, Vector3 is statically sized. +#include +using gtsam::Vector; +using gtsam::Vector3; + +// Unknown landmarks are of type Point2 (which is just a 2D Eigen vector). +#include +using gtsam::Point2; + +// Each variable in the system (poses and landmarks) must be identified with a +// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols +// (X1, X2, L1). Here we will use Symbols. #include +using gtsam::Symbol; -// We want to use iSAM2 to solve the range-SLAM problem incrementally +// We want to use iSAM2 to solve the range-SLAM problem incrementally. #include -// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, -// and initial guesses for any new variables used in the added factors +// iSAM2 requires as input a set set of new factors to be added stored in a +// factor graph, and initial guesses for any new variables in the added factors. #include #include -// We will use a non-liear solver to batch-inituialize from the first 150 frames +// We will use a non-linear solver to batch-initialize from the first 150 frames #include -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics SLAM problems. -#include +// Measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics SLAM problems: #include +#include #include -// Standard headers, added last, so we know headers above work on their own +// Timing, with functions below, provides nice facilities to benchmark. +#include +using gtsam::tictoc_print_; + +// Standard headers, added last, so we know headers above work on their own. #include #include +#include +#include +#include +#include +#include -using namespace std; -using namespace gtsam; namespace NM = gtsam::noiseModel; -// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/ -// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html) +// Data is second UWB ranging dataset, B2 or "plaza 2", from +// "Navigating with Ranging Radios: Five Data Sets with Ground Truth" +// by Joseph Djugash, Bradley Hamner, and Stephan Roth +// https://www.ri.cmu.edu/pub_files/2009/9/Final_5datasetsRangingRadios.pdf // load the odometry // DR: Odometry Input (delta distance traveled and delta heading change) -// Time (sec) Delta Dist. Trav. (m) Delta Heading (rad) -typedef pair TimedOdometry; -list readOdometry() { - list odometryList; - string data_file = findExampleDataFile("Plaza2_DR.txt"); - ifstream is(data_file.c_str()); +// Time (sec) Delta Distance Traveled (m) Delta Heading (rad) +using TimedOdometry = std::pair; +std::list readOdometry() { + std::list odometryList; + std::string data_file = gtsam::findExampleDataFile("Plaza2_DR.txt"); + std::ifstream is(data_file.c_str()); while (is) { double t, distance_traveled, delta_heading; is >> t >> distance_traveled >> delta_heading; - odometryList.push_back( - TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading))); + odometryList.emplace_back(t, Pose2(distance_traveled, 0, delta_heading)); } is.clear(); /* clears the end-of-file and error flags */ return odometryList; @@ -73,90 +92,85 @@ list readOdometry() { // load the ranges from TD // Time (sec) Sender / Antenna ID Receiver Node ID Range (m) -typedef boost::tuple RangeTriple; -vector readTriples() { - vector triples; - string data_file = findExampleDataFile("Plaza2_TD.txt"); - ifstream is(data_file.c_str()); +using RangeTriple = boost::tuple; +std::vector readTriples() { + std::vector triples; + std::string data_file = gtsam::findExampleDataFile("Plaza2_TD.txt"); + std::ifstream is(data_file.c_str()); while (is) { - double t, sender, range; - size_t receiver; + double t, range, sender, receiver; is >> t >> sender >> receiver >> range; - triples.push_back(RangeTriple(t, receiver, range)); + triples.emplace_back(t, receiver, range); } is.clear(); /* clears the end-of-file and error flags */ return triples; } // main -int main (int argc, char** argv) { - +int main(int argc, char** argv) { // load Plaza2 data - list odometry = readOdometry(); -// size_t M = odometry.size(); + std::list odometry = readOdometry(); + size_t M = odometry.size(); + std::cout << "Read " << M << " odometry entries." << std::endl; - vector triples = readTriples(); + std::vector triples = readTriples(); size_t K = triples.size(); + std::cout << "Read " << K << " range triples." << std::endl; // parameters - size_t minK = 150; // minimum number of range measurements to process initially - size_t incK = 25; // minimum number of range measurements to process after - bool groundTruth = false; + size_t minK = + 150; // minimum number of range measurements to process initially + size_t incK = 25; // minimum number of range measurements to process after bool robust = true; // Set Noise parameters - Vector priorSigmas = Vector3(1,1,M_PI); + Vector priorSigmas = Vector3(1, 1, M_PI); Vector odoSigmas = Vector3(0.05, 0.01, 0.1); - double sigmaR = 100; // range standard deviation - const NM::Base::shared_ptr // all same type - priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior - odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry - gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust - tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust - rangeNoise = robust ? tukey : gaussian; + double sigmaR = 100; // range standard deviation + const NM::Base::shared_ptr // all same type + priorNoise = NM::Diagonal::Sigmas(priorSigmas), // prior + looseNoise = NM::Isotropic::Sigma(2, 1000), // loose LM prior + odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry + gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust + tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), + gaussian), // robust + rangeNoise = robust ? tukey : gaussian; // Initialize iSAM - ISAM2 isam; + gtsam::ISAM2 isam; // Add prior on first pose - Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, - M_PI - 2.02108900000000); - NonlinearFactorGraph newFactors; + Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.021089); + gtsam::NonlinearFactorGraph newFactors; newFactors.addPrior(0, pose0, priorNoise); - Values initial; + gtsam::Values initial; initial.insert(0, pose0); - // initialize points - if (groundTruth) { // from TL file - initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778)); - initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278)); - initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678)); - initial.insert(symbol('L', 5), Point2(1.7095, -5.8122)); - } else { // drawn from sigma=1 Gaussian in matlab version - initial.insert(symbol('L', 1), Point2(3.5784, 2.76944)); - initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492)); - initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549)); - initial.insert(symbol('L', 5), Point2(0.714743, -0.204966)); - } + // We will initialize landmarks randomly, and keep track of which landmarks we + // already added with a set. + std::mt19937_64 rng; + std::normal_distribution normal(0.0, 100.0); + std::set initializedLandmarks; // set some loop variables - size_t i = 1; // step counter - size_t k = 0; // range measurement counter + size_t i = 1; // step counter + size_t k = 0; // range measurement counter bool initialized = false; Pose2 lastPose = pose0; size_t countK = 0; // Loop over odometry gttic_(iSAM); - for(const TimedOdometry& timedOdometry: odometry) { - //--------------------------------- odometry loop ----------------------------------------- + for (const TimedOdometry& timedOdometry : odometry) { + //--------------------------------- odometry loop -------------------------- double t; Pose2 odometry; boost::tie(t, odometry) = timedOdometry; // add odometry factor - newFactors.push_back(BetweenFactor(i-1, i, odometry, odoNoise)); + newFactors.emplace_shared>(i - 1, i, odometry, + odoNoise); // predict pose and add as initial estimate Pose2 predictedPose = lastPose.compose(odometry); @@ -166,17 +180,30 @@ int main (int argc, char** argv) { // Check if there are range factors to be added while (k < K && t >= boost::get<0>(triples[k])) { size_t j = boost::get<1>(triples[k]); + Symbol landmark_key('L', j); double range = boost::get<2>(triples[k]); - newFactors.push_back(RangeFactor(i, symbol('L', j), range,rangeNoise)); + newFactors.emplace_shared>( + i, landmark_key, range, rangeNoise); + if (initializedLandmarks.count(landmark_key) == 0) { + std::cout << "adding landmark " << j << std::endl; + double x = normal(rng), y = normal(rng); + initial.insert(landmark_key, Point2(x, y)); + initializedLandmarks.insert(landmark_key); + // We also add a very loose prior on the landmark in case there is only + // one sighting, which cannot fully determine the landmark. + newFactors.emplace_shared>( + landmark_key, Point2(0, 0), looseNoise); + } k = k + 1; countK = countK + 1; } // Check whether to update iSAM 2 if ((k > minK) && (countK > incK)) { - if (!initialized) { // Do a full optimize for first minK ranges + if (!initialized) { // Do a full optimize for first minK ranges + std::cout << "Initializing at time " << k << std::endl; gttic_(batchInitialization); - LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial); + gtsam::LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial); initial = batchOptimizer.optimize(); gttoc_(batchInitialization); initialized = true; @@ -185,21 +212,27 @@ int main (int argc, char** argv) { isam.update(newFactors, initial); gttoc_(update); gttic_(calculateEstimate); - Values result = isam.calculateEstimate(); + gtsam::Values result = isam.calculateEstimate(); gttoc_(calculateEstimate); lastPose = result.at(i); - newFactors = NonlinearFactorGraph(); - initial = Values(); + newFactors = gtsam::NonlinearFactorGraph(); + initial = gtsam::Values(); countK = 0; } i += 1; - //--------------------------------- odometry loop ----------------------------------------- - } // end for + //--------------------------------- odometry loop -------------------------- + } // end for gttoc_(iSAM); // Print timings tictoc_print_(); + // Print optimized landmarks: + gtsam::Values finalResult = isam.calculateEstimate(); + for (auto&& landmark_key : initializedLandmarks) { + Point2 p = finalResult.at(landmark_key); + std::cout << landmark_key << ":" << p.transpose() << "\n"; + } + exit(0); } - diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 3a02e6cab..8a5a12e56 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -26,10 +26,12 @@ #include // Header order is close to far -#include -#include #include // for loading BAL datasets ! #include +#include +#include + +#include #include using namespace std; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 6944177c1..10563760d 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -16,12 +16,14 @@ */ // For an explanation of headers, see SFMExample.cpp -#include +#include // for loading BAL datasets ! +#include +#include #include #include -#include -#include // for loading BAL datasets ! -#include +#include + +#include #include using namespace std; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 4d04dd16e..92d779a56 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -17,16 +17,16 @@ */ // For an explanation of headers, see SFMExample.cpp -#include -#include -#include -#include #include #include // for loading BAL datasets ! #include - +#include +#include +#include +#include #include +#include #include using namespace std; diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 04d3c9e47..3031828f1 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -22,6 +22,8 @@ * Passing function argument allows to specificy an initial position, a pose increment and step count. */ +#pragma once + // As this is a full 3D problem, we will use Pose3 variables to represent the camera // positions and Point3 variables (x, y, z) to represent the landmark coordinates. // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). @@ -66,4 +68,4 @@ std::vector createPoses( } return poses; -} \ No newline at end of file +} diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index a5ee77495..6fe2d06e3 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,6 +18,7 @@ #pragma once +#include #if BOOST_VERSION >= 107400 #include #endif diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 41a80629b..5b8a021d4 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 61c61a5af..cfedf6d8c 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -26,12 +26,9 @@ #include #include -#include - -#include -#include #include -#include + +#include /** * Matrix is a typedef in the gtsam namespace @@ -523,82 +520,4 @@ GTSAM_EXPORT Matrix LLt(const Matrix& A); GTSAM_EXPORT Matrix RtR(const Matrix& A); GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); -} // namespace gtsam - -#include -#include -#include - -namespace boost { - namespace serialization { - - /** - * Ref. https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063 - * - * Eigen supports calling resize() on both static and dynamic matrices. - * This allows for a uniform API, with resize having no effect if the static matrix - * is already the correct size. - * https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing - * - * We use all the Matrix template parameters to ensure wide compatibility. - * - * eigen_typekit in ROS uses the same code - * http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html - */ - - // split version - sends sizes ahead - template - void save(Archive & ar, - const Eigen::Matrix & m, - const unsigned int /*version*/) { - const size_t rows = m.rows(), cols = m.cols(); - ar << BOOST_SERIALIZATION_NVP(rows); - ar << BOOST_SERIALIZATION_NVP(cols); - ar << make_nvp("data", make_array(m.data(), m.size())); - } - - template - void load(Archive & ar, - Eigen::Matrix & m, - const unsigned int /*version*/) { - size_t rows, cols; - ar >> BOOST_SERIALIZATION_NVP(rows); - ar >> BOOST_SERIALIZATION_NVP(cols); - m.resize(rows, cols); - ar >> make_nvp("data", make_array(m.data(), m.size())); - } - - // templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix); - template - void serialize(Archive & ar, - Eigen::Matrix & m, - const unsigned int version) { - split_free(ar, m, version); - } - - // specialized to Matrix for MATLAB wrapper - template - void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) { - split_free(ar, m, version); - } - - } // namespace serialization -} // namespace boost +} // namespace gtsam diff --git a/gtsam/base/MatrixSerialization.h b/gtsam/base/MatrixSerialization.h new file mode 100644 index 000000000..f79d7b27f --- /dev/null +++ b/gtsam/base/MatrixSerialization.h @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * 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 MatrixSerialization.h + * @brief Serialization for matrices + * @author Frank Dellaert + * @date February 2022 + */ + +// \callgraph + +#pragma once + +#include + +#include +#include +#include + +namespace boost { +namespace serialization { + +/** + * Ref. + * https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063 + * + * Eigen supports calling resize() on both static and dynamic matrices. + * This allows for a uniform API, with resize having no effect if the static + * matrix is already the correct size. + * https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing + * + * We use all the Matrix template parameters to ensure wide compatibility. + * + * eigen_typekit in ROS uses the same code + * http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html + */ + +// split version - sends sizes ahead +template +void save( + Archive& ar, + const Eigen::Matrix& m, + const unsigned int /*version*/) { + const size_t rows = m.rows(), cols = m.cols(); + ar << BOOST_SERIALIZATION_NVP(rows); + ar << BOOST_SERIALIZATION_NVP(cols); + ar << make_nvp("data", make_array(m.data(), m.size())); +} + +template +void load(Archive& ar, + Eigen::Matrix& m, + const unsigned int /*version*/) { + size_t rows, cols; + ar >> BOOST_SERIALIZATION_NVP(rows); + ar >> BOOST_SERIALIZATION_NVP(cols); + m.resize(rows, cols); + ar >> make_nvp("data", make_array(m.data(), m.size())); +} + +// templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix); +template +void serialize( + Archive& ar, + Eigen::Matrix& m, + const unsigned int version) { + split_free(ar, m, version); +} + +// specialized to Matrix for MATLAB wrapper +template +void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) { + split_free(ar, m, version); +} + +} // namespace serialization +} // namespace boost diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index a19fbe176..697c4f3be 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -21,6 +21,7 @@ #include // Configuration from CMake #include +#include #include #include diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 36dc2288d..9cb2aa165 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -264,46 +264,4 @@ GTSAM_EXPORT Vector concatVectors(const std::list& vs); * concatenate Vectors */ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); -} // namespace gtsam - -#include -#include -#include - -namespace boost { - namespace serialization { - - // split version - copies into an STL vector for serialization - template - void save(Archive & ar, const gtsam::Vector & v, unsigned int /*version*/) { - const size_t size = v.size(); - ar << BOOST_SERIALIZATION_NVP(size); - ar << make_nvp("data", make_array(v.data(), v.size())); - } - - template - void load(Archive & ar, gtsam::Vector & v, unsigned int /*version*/) { - size_t size; - ar >> BOOST_SERIALIZATION_NVP(size); - v.resize(size); - ar >> make_nvp("data", make_array(v.data(), v.size())); - } - - // split version - copies into an STL vector for serialization - template - void save(Archive & ar, const Eigen::Matrix & v, unsigned int /*version*/) { - ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); - } - - template - void load(Archive & ar, Eigen::Matrix & v, unsigned int /*version*/) { - ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); - } - - } // namespace serialization -} // namespace boost - -BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector) -BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2) -BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3) -BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6) +} // namespace gtsam diff --git a/gtsam/base/VectorSerialization.h b/gtsam/base/VectorSerialization.h new file mode 100644 index 000000000..97df02a75 --- /dev/null +++ b/gtsam/base/VectorSerialization.h @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * 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 VectorSerialization.h + * @brief serialization for Vectors + * @author Frank Dellaert + * @date February 2022 + */ + +#pragma once + +#include + +#include +#include +#include + +namespace boost { +namespace serialization { + +// split version - copies into an STL vector for serialization +template +void save(Archive& ar, const gtsam::Vector& v, unsigned int /*version*/) { + const size_t size = v.size(); + ar << BOOST_SERIALIZATION_NVP(size); + ar << make_nvp("data", make_array(v.data(), v.size())); +} + +template +void load(Archive& ar, gtsam::Vector& v, unsigned int /*version*/) { + size_t size; + ar >> BOOST_SERIALIZATION_NVP(size); + v.resize(size); + ar >> make_nvp("data", make_array(v.data(), v.size())); +} + +// split version - copies into an STL vector for serialization +template +void save(Archive& ar, const Eigen::Matrix& v, + unsigned int /*version*/) { + ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); +} + +template +void load(Archive& ar, Eigen::Matrix& v, + unsigned int /*version*/) { + ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); +} + +} // namespace serialization +} // namespace boost + +BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector) +BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2) +BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3) +BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6) diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 92031db2b..0d8d69df8 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include namespace gtsam { diff --git a/gtsam/base/base.i b/gtsam/base/base.i index 9838f97d3..9b9f351ce 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -82,6 +82,7 @@ class IndexPairSetMap { }; #include +#include bool linear_independent(Matrix A, Matrix B, double tol); #include diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 5e3276ff0..bf7d18a1d 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -18,7 +18,6 @@ #pragma once #include -#include namespace gtsam { diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 5994a5e51..bb8574245 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -42,7 +42,7 @@ T create() { } // Creates or empties a folder in the build folder and returns the relative path -boost::filesystem::path resetFilesystem( +inline boost::filesystem::path resetFilesystem( boost::filesystem::path folder = "actual") { boost::filesystem::remove_all(folder); boost::filesystem::create_directory(folder); diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index d863eaba3..f7aa97b31 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index d8bd28c1a..765a2f645 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -92,7 +92,7 @@ Matrix kroneckerProductIdentity(const Weights& w) { /// CRTP Base class for function bases template -class GTSAM_EXPORT Basis { +class Basis { public: /** * Calculate weights for all x in vector X. @@ -497,11 +497,6 @@ class GTSAM_EXPORT Basis { } }; - // Vector version for MATLAB :-( - static double Derivative(double x, const Vector& p, // - OptionalJacobian H = boost::none) { - return DerivativeFunctor(x)(p.transpose(), H); - } }; } // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 0b3d4c1a0..648bcd510 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -29,9 +29,12 @@ namespace gtsam { * pseudo-spectral parameterization. * * @tparam BASIS The basis class to use e.g. Chebyshev2 + * + * Example, degree 8 Chebyshev polynomial measured at x=0.5: + * EvaluationFactor factor(key, measured, model, 8, 0.5); */ template -class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { +class EvaluationFactor : public FunctorizedFactor { private: using Base = FunctorizedFactor; @@ -47,7 +50,7 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { * @param N The degree of the polynomial. * @param x The point at which to evaluate the polynomial. */ - EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x) : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} @@ -62,7 +65,7 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { * @param a Lower bound for the polynomial. * @param b Upper bound for the polynomial. */ - EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x, double a, double b) : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} @@ -85,7 +88,7 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { * @param M: Size of the evaluated state vector. */ template -class GTSAM_EXPORT VectorEvaluationFactor +class VectorEvaluationFactor : public FunctorizedFactor> { private: using Base = FunctorizedFactor>; @@ -148,7 +151,7 @@ class GTSAM_EXPORT VectorEvaluationFactor * where N is the degree and i is the component index. */ template -class GTSAM_EXPORT VectorComponentFactor +class VectorComponentFactor : public FunctorizedFactor> { private: using Base = FunctorizedFactor>; @@ -217,7 +220,7 @@ class GTSAM_EXPORT VectorComponentFactor * where `x` is the value (e.g. timestep) at which the rotation was evaluated. */ template -class GTSAM_EXPORT ManifoldEvaluationFactor +class ManifoldEvaluationFactor : public FunctorizedFactor::dimension>> { private: using Base = FunctorizedFactor::dimension>>; @@ -269,7 +272,7 @@ class GTSAM_EXPORT ManifoldEvaluationFactor * @param BASIS: The basis class to use e.g. Chebyshev2 */ template -class GTSAM_EXPORT DerivativeFactor +class DerivativeFactor : public FunctorizedFactor { private: using Base = FunctorizedFactor; @@ -318,7 +321,7 @@ class GTSAM_EXPORT DerivativeFactor * @param M: Size of the evaluated state vector derivative. */ template -class GTSAM_EXPORT VectorDerivativeFactor +class VectorDerivativeFactor : public FunctorizedFactor> { private: using Base = FunctorizedFactor>; @@ -371,7 +374,7 @@ class GTSAM_EXPORT VectorDerivativeFactor * @param P: Size of the control component derivative. */ template -class GTSAM_EXPORT ComponentDerivativeFactor +class ComponentDerivativeFactor : public FunctorizedFactor> { private: using Base = FunctorizedFactor>; diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index d16ccfaac..1c16c47bf 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -21,8 +21,6 @@ #include #include -#include - namespace gtsam { /** @@ -31,7 +29,7 @@ namespace gtsam { * These are typically denoted with the symbol T_n, where n is the degree. * The parameter N is the number of coefficients, i.e., N = n+1. */ -struct Chebyshev1Basis : Basis { +struct GTSAM_EXPORT Chebyshev1Basis : Basis { using Parameters = Eigen::Matrix; Parameters parameters_; @@ -79,7 +77,7 @@ struct Chebyshev1Basis : Basis { * functions. In this sense, they are like the sines and cosines of the Fourier * basis. */ -struct Chebyshev2Basis : Basis { +struct GTSAM_EXPORT Chebyshev2Basis : Basis { using Parameters = Eigen::Matrix; /** diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 28590961d..e306c93d5 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -22,8 +22,7 @@ * * This is different from Chebyshev.h since it leverage ideas from * pseudo-spectral optimization, i.e. we don't decompose into basis functions, - * rather estimate function parameters that enforce function nodes at Chebyshev - * points. + * rather estimate function values at the Chebyshev points. * * Please refer to Agrawal21icra for more details. * diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index d264e182d..eb259bd8a 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -24,7 +24,7 @@ namespace gtsam { /// Fourier basis -class GTSAM_EXPORT FourierBasis : public Basis { +class FourierBasis : public Basis { public: using Parameters = Eigen::Matrix; using DiffMatrix = Eigen::Matrix; diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index c9c027438..a6c9d87ee 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -44,9 +44,6 @@ class Chebyshev2 { static Matrix DerivativeWeights(size_t N, double x, double a, double b); static Matrix IntegrationWeights(size_t N, double a, double b); static Matrix DifferentiationMatrix(size_t N, double a, double b); - - // TODO Needs OptionalJacobian - // static double Derivative(double x, Vector f); }; #include diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp new file mode 100644 index 000000000..18a389da5 --- /dev/null +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -0,0 +1,230 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- + */ + +/** + * @file testBasisFactors.cpp + * @date May 31, 2020 + * @author Varun Agrawal + * @brief unit tests for factors in BasisFactors.h + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using gtsam::noiseModel::Isotropic; +using gtsam::Pose2; +using gtsam::Vector; +using gtsam::Values; +using gtsam::Chebyshev2; +using gtsam::ParameterMatrix; +using gtsam::LevenbergMarquardtParams; +using gtsam::LevenbergMarquardtOptimizer; +using gtsam::NonlinearFactorGraph; +using gtsam::NonlinearOptimizerParams; + +constexpr size_t N = 2; + +// Key used in all tests +const gtsam::Symbol key('X', 0); + +//****************************************************************************** +TEST(BasisFactors, EvaluationFactor) { + using gtsam::EvaluationFactor; + + double measured = 0; + + auto model = Isotropic::Sigma(1, 1.0); + EvaluationFactor factor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(factor); + + Vector functionValues(N); + functionValues.setZero(); + + Values initial; + initial.insert(key, functionValues); + + LevenbergMarquardtParams parameters; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(BasisFactors, VectorEvaluationFactor) { + using gtsam::VectorEvaluationFactor; + const size_t M = 4; + + const Vector measured = Vector::Zero(M); + + auto model = Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor factor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(factor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(BasisFactors, Print) { + using gtsam::VectorEvaluationFactor; + const size_t M = 1; + + const Vector measured = Vector::Ones(M) * 42; + + auto model = Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor factor(key, measured, model, N, 0); + + std::string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, factor)); +} + +//****************************************************************************** +TEST(BasisFactors, VectorComponentFactor) { + using gtsam::VectorComponentFactor; + const int P = 4; + const size_t i = 2; + const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; + auto model = Isotropic::Sigma(1, 1.0); + VectorComponentFactor factor(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(factor); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(BasisFactors, ManifoldEvaluationFactor) { + using gtsam::ManifoldEvaluationFactor; + const Pose2 measured; + const double t = 3.0, a = 2.0, b = 4.0; + auto model = Isotropic::Sigma(3, 1.0); + ManifoldEvaluationFactor factor(key, measured, model, N, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(factor); + + ParameterMatrix<3> stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(BasisFactors, VecDerivativePrior) { + using gtsam::VectorDerivativeFactor; + const size_t M = 4; + + const Vector measured = Vector::Zero(M); + auto model = Isotropic::Sigma(M, 1.0); + VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(BasisFactors, ComponentDerivativeFactor) { + using gtsam::ComponentDerivativeFactor; + const size_t M = 4; + + double measured = 0; + auto model = Isotropic::Sigma(1, 1.0); + ComponentDerivativeFactor controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp index 64c925886..7d7f9323d 100644 --- a/gtsam/basis/tests/testChebyshev.cpp +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -25,9 +25,10 @@ using namespace std; using namespace gtsam; +namespace { auto model = noiseModel::Unit::Create(1); - const size_t N = 3; +} // namespace //****************************************************************************** TEST(Chebyshev, Chebyshev1) { diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 4cee70daf..9090757f4 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -10,26 +10,30 @@ * -------------------------------------------------------------------------- */ /** - * @file testChebyshev.cpp + * @file testChebyshev2.cpp * @date July 4, 2020 * @author Varun Agrawal * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral * methods */ -#include -#include #include #include +#include #include +#include + +#include using namespace std; using namespace gtsam; using namespace boost::placeholders; +namespace { noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); const size_t N = 32; +} // namespace //****************************************************************************** TEST(Chebyshev2, Point) { @@ -121,12 +125,30 @@ TEST(Chebyshev2, InterpolateVector) { EXPECT(assert_equal(numericalH, actualH, 1e-9)); } +//****************************************************************************** +// Interpolating poses using the exponential map +TEST(Chebyshev2, InterpolatePose2) { + double t = 30, a = 0, b = 100; + + ParameterMatrix<3> X(N); + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + X.row(1) = Vector::Zero(N); + X.row(2) = 0.1 * Vector::Ones(N); + + Vector xi(3); + xi << t, 0, 0.1; + Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); + // We use xi as canonical coordinates via exponential map + Pose2 expected = Pose2::ChartAtOrigin::Retract(xi); + EXPECT(assert_equal(expected, fx(X))); +} + //****************************************************************************** TEST(Chebyshev2, Decomposition) { // Create example sequence Sequence sequence; for (size_t i = 0; i < 16; i++) { - double x = (double)i / 16. - 0.99, y = x; + double x = (1.0/ 16)*i - 0.99, y = x; sequence[x] = y; } @@ -144,11 +166,11 @@ TEST(Chebyshev2, DifferentiationMatrix3) { // Trefethen00book, p.55 const size_t N = 3; Matrix expected(N, N); - // Differentiation matrix computed from Chebfun + // Differentiation matrix computed from chebfun expected << 1.5000, -2.0000, 0.5000, // 0.5000, -0.0000, -0.5000, // -0.5000, 2.0000, -1.5000; - // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // multiply by -1 since the chebyshev points have a phase shift wrt Trefethen // This was verified with chebfun expected = -expected; @@ -167,7 +189,7 @@ TEST(Chebyshev2, DerivativeMatrix6) { 0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; - // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // multiply by -1 since the chebyshev points have a phase shift wrt Trefethen // This was verified with chebfun expected = -expected; @@ -252,7 +274,7 @@ TEST(Chebyshev2, DerivativeWeights2) { Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); - // test if derivative calculation and cheb point is correct + // test if derivative calculation and Chebyshev point is correct double x3 = Chebyshev2::Point(N, 3, a, b); Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); diff --git a/gtsam/discrete/AlgebraicDecisionTree.cpp b/gtsam/discrete/AlgebraicDecisionTree.cpp new file mode 100644 index 000000000..83ee4051a --- /dev/null +++ b/gtsam/discrete/AlgebraicDecisionTree.cpp @@ -0,0 +1,28 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file AlgebraicDecisionTree.cpp + * @date Feb 20, 2022 + * @author Mike Sheffler + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include "AlgebraicDecisionTree.h" + +#include + +namespace gtsam { + + template class AlgebraicDecisionTree; + +} // namespace gtsam diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 828f0b1a2..a2ceac834 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -127,7 +127,7 @@ namespace gtsam { return map.at(label); }; std::function op = Ring::id; - this->root_ = this->template convertFrom(other.root_, L_of_M, op); + this->root_ = DecisionTree::convertFrom(other.root_, L_of_M, op); } /** sum */ diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h index cdbf0a2e9..90e2dbdd8 100644 --- a/gtsam/discrete/Assignment.h +++ b/gtsam/discrete/Assignment.h @@ -33,6 +33,8 @@ namespace gtsam { template class Assignment : public std::map { public: + using std::map::operator=; + void print(const std::string& s = "Assignment: ") const { std::cout << s << ": "; for (const typename Assignment::value_type& keyValue : *this) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 01c7b689c..b6e548297 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -112,6 +112,13 @@ namespace gtsam { return f; } + /// Apply unary operator with assignment + NodePtr apply(const UnaryAssignment& op, + const Assignment& choices) const override { + NodePtr f(new Leaf(op(choices, constant_))); + return f; + } + // Apply binary operator "h = f op g" on Leaf node // Note op is not assumed commutative so we need to keep track of order // Simply calls apply on argument to call correct virtual method: @@ -322,12 +329,48 @@ namespace gtsam { for (const NodePtr& branch : f.branches_) push_back(branch->apply(op)); } + /** + * @brief Constructor which accepts a UnaryAssignment op and the + * corresponding assignment. + * + * @param label The label for this node. + * @param f The original choice node to apply the op on. + * @param op Function to apply on the choice node. Takes Assignment and + * value as arguments. + * @param choices The Assignment that will go to op. + */ + Choice(const L& label, const Choice& f, const UnaryAssignment& op, + const Assignment& choices) + : label_(label), allSame_(true) { + branches_.reserve(f.branches_.size()); // reserve space + + Assignment choices_ = choices; + + for (size_t i = 0; i < f.branches_.size(); i++) { + choices_[label_] = i; // Set assignment for label to i + + const NodePtr branch = f.branches_[i]; + push_back(branch->apply(op, choices_)); + + // Remove the choice so we are backtracking + auto choice_it = choices_.find(label_); + choices_.erase(choice_it); + } + } + /** apply unary operator */ NodePtr apply(const Unary& op) const override { auto r = boost::make_shared(label_, *this, op); return Unique(r); } + /// Apply unary operator with assignment + NodePtr apply(const UnaryAssignment& op, + const Assignment& choices) const override { + auto r = boost::make_shared(label_, *this, op, choices); + return Unique(r); + } + // Apply binary operator "h = f op g" on Choice node // Note op is not assumed commutative so we need to keep track of order // Simply calls apply on argument to call correct virtual method: @@ -592,11 +635,13 @@ namespace gtsam { std::function Y_of_X) const { using LY = DecisionTree; - // ugliness below because apparently we can't have templated virtual - // functions If leaf, apply unary conversion "op" and create a unique leaf + // Ugliness below because apparently we can't have templated virtual + // functions. + // If leaf, apply unary conversion "op" and create a unique leaf. using MXLeaf = typename DecisionTree::Leaf; - if (auto leaf = boost::dynamic_pointer_cast(f)) + if (auto leaf = boost::dynamic_pointer_cast(f)) { return NodePtr(new Leaf(Y_of_X(leaf->constant()))); + } // Check if Choice using MXChoice = typename DecisionTree::Choice; @@ -666,8 +711,13 @@ namespace gtsam { if (!choice) throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr"); for (size_t i = 0; i < choice->nrChoices(); i++) { - choices[choice->label()] = i; // Set assignment for label to i + choices[choice->label()] = i; // Set assignment for label to i + (*this)(choice->branches()[i]); // recurse! + + // Remove the choice so we are backtracking + auto choice_it = choices.find(choice->label()); + choices.erase(choice_it); } } }; @@ -679,6 +729,14 @@ namespace gtsam { visit(root_); } + /****************************************************************************/ + template + size_t DecisionTree::nrLeaves() const { + size_t total = 0; + visit([&total](const Y& node) { total += 1; }); + return total; + } + /****************************************************************************/ // fold is just done with a visit template @@ -734,6 +792,19 @@ namespace gtsam { return DecisionTree(root_->apply(op)); } + /// Apply unary operator with assignment + template + DecisionTree DecisionTree::apply( + const UnaryAssignment& op) const { + // It is unclear what should happen if tree is empty: + if (empty()) { + throw std::runtime_error( + "DecisionTree::apply(unary op) undefined for empty tree."); + } + Assignment choices; + return DecisionTree(root_->apply(op, choices)); + } + /****************************************************************************/ template DecisionTree DecisionTree::apply(const DecisionTree& g, diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index d655756b8..c0a2a7a1c 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -54,6 +54,7 @@ namespace gtsam { /** Handy typedefs for unary and binary function types */ using Unary = std::function; + using UnaryAssignment = std::function&, const Y&)>; using Binary = std::function; /** A label annotated with cardinality */ @@ -103,6 +104,8 @@ namespace gtsam { &DefaultCompare) const = 0; virtual const Y& operator()(const Assignment& x) const = 0; virtual Ptr apply(const Unary& op) const = 0; + virtual Ptr apply(const UnaryAssignment& op, + const Assignment& choices) const = 0; virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0; virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0; @@ -259,6 +262,9 @@ namespace gtsam { template void visitWith(Func f) const; + /// Return the number of leaves in the tree. + size_t nrLeaves() const; + /** * @brief Fold a binary function over the tree, returning accumulator. * @@ -283,6 +289,16 @@ namespace gtsam { /** apply Unary operation "op" to f */ DecisionTree apply(const Unary& op) const; + /** + * @brief Apply Unary operation "op" to f while also providing the + * corresponding assignment. + * + * @param op Function which takes Assignment and Y as input and returns + * object of type Y. + * @return DecisionTree + */ + DecisionTree apply(const UnaryAssignment& op) const; + /** apply binary operation "op" to f and g */ DecisionTree apply(const DecisionTree& g, const Binary& op) const; @@ -337,6 +353,13 @@ namespace gtsam { return f.apply(op); } + /// Apply unary operator `op` with Assignment to DecisionTree `f`. + template + DecisionTree apply(const DecisionTree& f, + const typename DecisionTree::UnaryAssignment& op) { + return f.apply(op); + } + /// Apply binary operator `op` to DecisionTree `f`. template DecisionTree apply(const DecisionTree& f, diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index ef4cc48f6..e95b8fe37 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -156,10 +156,7 @@ namespace gtsam { std::vector> DecisionTreeFactor::enumerate() const { // Get all possible assignments - std::vector> pairs; - for (auto& key : keys()) { - pairs.emplace_back(key, cardinalities_.at(key)); - } + std::vector> pairs = discreteKeys(); // Reverse to make cartesian product output a more natural ordering. std::vector> rpairs(pairs.rbegin(), pairs.rend()); const auto assignments = DiscreteValues::CartesianProduct(rpairs); diff --git a/gtsam/discrete/DiscreteJunctionTree.h b/gtsam/discrete/DiscreteJunctionTree.h index f5bc9be1d..c74ad3cc2 100644 --- a/gtsam/discrete/DiscreteJunctionTree.h +++ b/gtsam/discrete/DiscreteJunctionTree.h @@ -16,6 +16,8 @@ * @author Richard Roberts */ +#pragma once + #include #include #include diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index ce0c56dbe..dea00074d 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -72,5 +72,5 @@ namespace gtsam { }; // DiscreteKeys /// Create a list from two keys - DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2); + GTSAM_EXPORT DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2); } diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index 8cb651f28..15169a1dc 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -36,7 +36,7 @@ class DiscreteBayesNet; * Inherits from discrete conditional for convenience, but is not normalized. * Is used in the max-product algorithm. */ -class DiscreteLookupTable : public DiscreteConditional { +class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional { public: using This = DiscreteLookupTable; using shared_ptr = boost::shared_ptr; @@ -46,7 +46,7 @@ class DiscreteLookupTable : public DiscreteConditional { * @brief Construct a new Discrete Lookup Table object * * @param nFrontals number of frontal variables - * @param keys a orted list of gtsam::Keys + * @param keys a sorted list of gtsam::Keys * @param potentials the algebraic decision tree with lookup values */ DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys, diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index a2207a10b..dc87f665e 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -29,7 +29,7 @@ namespace gtsam { /** * A class for computing marginals of variables in a DiscreteFactorGraph */ -class GTSAM_EXPORT DiscreteMarginals { +class DiscreteMarginals { protected: diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h index 81997a783..cb17bf833 100644 --- a/gtsam/discrete/DiscreteValues.h +++ b/gtsam/discrete/DiscreteValues.h @@ -37,7 +37,7 @@ namespace gtsam { * stores cardinality of a Discrete variable. It should be handled naturally in * the new class DiscreteValue, as the variable's type (domain) */ -class DiscreteValues : public Assignment { +class GTSAM_EXPORT DiscreteValues : public Assignment { public: using Base = Assignment; // base class diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 9d130a1f6..c800321d6 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -318,7 +318,7 @@ TEST(ADT, factor_graph) { dot(fg, "Marginalized-3E"); fg = fg.combine(L, &add_); dot(fg, "Marginalized-2L"); - EXPECT(adds = 54); + LONGS_EQUAL(49, adds); gttoc_(marg); tictoc_getNode(margNode, marg); elapsed = margNode->secs() + margNode->wall(); diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index dbfb2dc40..f234905e3 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -17,17 +17,19 @@ * @date Jan 30, 2012 */ -#include -using namespace boost::assign; - -#include -#include -#include - // #define DT_DEBUG_MEMORY // #define DT_NO_PRUNING #define DISABLE_DOT #include + +#include +#include + +#include + +#include +using namespace boost::assign; + using namespace std; using namespace gtsam; @@ -88,6 +90,7 @@ struct DT : public DecisionTree { auto valueFormatter = [](const int& v) { return (boost::format("%d") % v).str(); }; + std::cout << s; Base::print("", keyFormatter, valueFormatter); } /// Equality method customized to int node type @@ -148,9 +151,9 @@ TEST(DecisionTree, example) { DOT(notb); // Check supplying empty trees yields an exception - CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error); - CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error); - CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error); + CHECK_EXCEPTION(gtsam::apply(empty, &Ring::id), std::runtime_error); + CHECK_EXCEPTION(gtsam::apply(empty, a, &Ring::mul), std::runtime_error); + CHECK_EXCEPTION(gtsam::apply(a, empty, &Ring::mul), std::runtime_error); // apply, two nodes, in natural order DT anotb = apply(a, notb, &Ring::mul); @@ -344,6 +347,44 @@ TEST(DecisionTree, visitWith) { EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9); } +/* ************************************************************************** */ +// Test visit, with Choices argument. +TEST(DecisionTree, VisitWithPruned) { + // Create pruned tree + std::pair A("A", 2), B("B", 2), C("C", 2); + std::vector> labels = {C, B, A}; + std::vector nodes = {0, 0, 2, 3, 4, 4, 6, 7}; + DT tree(labels, nodes); + + std::vector> choices; + auto func = [&](const Assignment& choice, const int& d) { + choices.push_back(choice); + }; + tree.visitWith(func); + + EXPECT_LONGS_EQUAL(6, choices.size()); + + Assignment expectedAssignment; + + expectedAssignment = {{"B", 0}, {"C", 0}}; + EXPECT(expectedAssignment == choices.at(0)); + + expectedAssignment = {{"A", 0}, {"B", 1}, {"C", 0}}; + EXPECT(expectedAssignment == choices.at(1)); + + expectedAssignment = {{"A", 1}, {"B", 1}, {"C", 0}}; + EXPECT(expectedAssignment == choices.at(2)); + + expectedAssignment = {{"B", 0}, {"C", 1}}; + EXPECT(expectedAssignment == choices.at(3)); + + expectedAssignment = {{"A", 0}, {"B", 1}, {"C", 1}}; + EXPECT(expectedAssignment == choices.at(4)); + + expectedAssignment = {{"A", 1}, {"B", 1}, {"C", 1}}; + EXPECT(expectedAssignment == choices.at(5)); +} + /* ************************************************************************** */ // Test fold. TEST(DecisionTree, fold) { @@ -411,6 +452,43 @@ TEST(DecisionTree, threshold) { EXPECT_LONGS_EQUAL(2, thresholded.fold(count, 0)); } +/* ************************************************************************** */ +// Test apply with assignment. +TEST(DecisionTree, ApplyWithAssignment) { + // Create three level tree + vector keys; + keys += DT::LabelC("C", 2), DT::LabelC("B", 2), DT::LabelC("A", 2); + DT tree(keys, "1 2 3 4 5 6 7 8"); + + DecisionTree probTree( + keys, "0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08"); + double threshold = 0.045; + + // We test pruning one tree by indexing into another. + auto pruner = [&](const Assignment& choices, const int& x) { + // Prune out all the leaves with even numbers + if (probTree(choices) < threshold) { + return 0; + } else { + return x; + } + }; + DT prunedTree = tree.apply(pruner); + + DT expectedTree(keys, "0 0 0 0 5 6 7 8"); + EXPECT(assert_equal(expectedTree, prunedTree)); + + size_t count = 0; + auto counter = [&](const Assignment& choices, const int& x) { + count += 1; + return x; + }; + DT prunedTree2 = prunedTree.apply(counter); + + // Check if apply doesn't enumerate all leaves. + EXPECT_LONGS_EQUAL(5, count); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 92145b8b7..846653c38 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -107,7 +107,7 @@ TEST(DecisionTreeFactor, enumerate) { } /* ************************************************************************* */ -TEST(DiscreteFactorGraph, DotWithNames) { +TEST(DecisionTreeFactor, DotWithNames) { DiscreteKey A(12, 3), B(5, 2); DecisionTreeFactor f(A & B, "1 2 3 4 5 6"); auto formatter = [](Key key) { return key == 12 ? "A" : "B"; }; diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 8db7abffe..95b0232f0 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -22,6 +22,7 @@ #include #include #include +#include #include namespace gtsam { diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index affce0819..039497cc9 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -21,6 +21,7 @@ #pragma once #include +#include namespace gtsam { diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index b583234fd..1b2291e07 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -21,6 +21,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index a8c9fa182..c0caecaa1 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -22,6 +22,8 @@ #include #include +#include + #include namespace gtsam { diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 35578ffe0..065cd0140 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -15,6 +15,8 @@ * @author Frank Dellaert **/ +#pragma once + #include #include diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp index e3b4841e0..9e7b2e13e 100644 --- a/gtsam/geometry/Line3.cpp +++ b/gtsam/geometry/Line3.cpp @@ -117,4 +117,4 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL, return Line3(cRl, c_ab[0], c_ab[1]); } -} \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index f70e13ca7..78827274a 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -21,12 +21,27 @@ namespace gtsam { +class Line3; + +/** + * Transform a line from world to camera frame + * @param wTc - Pose3 of camera in world frame + * @param wL - Line3 in world frame + * @param Dpose - OptionalJacobian of transformed line with respect to p + * @param Dline - OptionalJacobian of transformed line with respect to l + * @return Transformed line in camera frame + */ +GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose = boost::none, + OptionalJacobian<4, 4> Dline = boost::none); + + /** * A 3D line (R,a,b) : (Rot3,Scalar,Scalar) * @addtogroup geometry * \nosubgrouping */ -class Line3 { +class GTSAM_EXPORT Line3 { private: Rot3 R_; // Rotation of line about x and y in world frame double a_, b_; // Intersection of line with the world x-y plane rotated by R_ @@ -136,18 +151,6 @@ class Line3 { OptionalJacobian<4, 4> Dline); }; -/** - * Transform a line from world to camera frame - * @param wTc - Pose3 of camera in world frame - * @param wL - Line3 in world frame - * @param Dpose - OptionalJacobian of transformed line with respect to p - * @param Dline - OptionalJacobian of transformed line with respect to l - * @return Transformed line in camera frame - */ -Line3 transformTo(const Pose3 &wTc, const Line3 &wL, - OptionalJacobian<4, 6> Dpose = boost::none, - OptionalJacobian<4, 4> Dline = boost::none); - template<> struct traits : public internal::Manifold {}; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 61e9f0909..c20e90819 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -30,7 +30,7 @@ namespace gtsam { * \nosubgrouping */ template -class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { +class PinholeCamera: public PinholeBaseK { public: @@ -230,13 +230,15 @@ public: Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera, OptionalJacobian<2, FixedDimension::value> Dpoint) const { // We just call 3-derivative version in Base - Matrix26 Dpose; - Eigen::Matrix Dcal; - Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint, - Dcamera ? &Dcal : 0); - if (Dcamera) + if (Dcamera){ + Matrix26 Dpose; + Eigen::Matrix Dcal; + const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal); *Dcamera << Dpose, Dcal; - return pi; + return pi; + } else { + return Base::project(pw, boost::none, Dpoint, boost::none); + } } /// project a 3D point from world coordinates into the image diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index b4999af7c..7b92be5d5 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -31,7 +31,7 @@ namespace gtsam { * \nosubgrouping */ template -class GTSAM_EXPORT PinholeBaseK: public PinholeBase { +class PinholeBaseK: public PinholeBase { private: diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index a565ac140..ef91108eb 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -17,6 +17,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index a0b3d502e..cc7f8e474 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -213,6 +213,14 @@ Point2 Pose2::transformTo(const Point2& point, return q; } +Matrix Pose2::transformTo(const Matrix& points) const { + if (points.rows() != 2) { + throw std::invalid_argument("Pose2:transformTo expects 2*N matrix."); + } + const Matrix2 Rt = rotation().transpose(); + return Rt * (points.colwise() - t_); // Eigen broadcasting! +} + /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transformFrom(const Point2& point, @@ -224,6 +232,15 @@ Point2 Pose2::transformFrom(const Point2& point, return q + t_; } + +Matrix Pose2::transformFrom(const Matrix& points) const { + if (points.rows() != 2) { + throw std::invalid_argument("Pose2:transformFrom expects 2*N matrix."); + } + const Matrix2 R = rotation().matrix(); + return (R * points).colwise() + t_; // Eigen broadcasting! +} + /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index a54951728..1e79836f5 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -199,13 +199,29 @@ public: OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const; + /** + * @brief transform many points in world coordinates and transform to Pose. + * @param points 2*N matrix in world coordinates + * @return points in Pose coordinates, as 2*N Matrix + */ + Matrix transformTo(const Matrix& points) const; + /** Return point coordinates in global frame */ GTSAM_EXPORT Point2 transformFrom(const Point2& point, OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const; + /** + * @brief transform many points in Pose coordinates and transform to world. + * @param points 2*N matrix in Pose coordinates + * @return points in world coordinates, as 2*N Matrix + */ + Matrix transformFrom(const Matrix& points) const; + /** syntactic sugar for transformFrom */ - inline Point2 operator*(const Point2& point) const { return transformFrom(point);} + inline Point2 operator*(const Point2& point) const { + return transformFrom(point); + } /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 1e399ad18..536947597 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -59,7 +59,7 @@ Matrix6 Pose3::AdjointMap() const { const Matrix3 R = R_.matrix(); Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; Matrix6 adj; - adj << R, Z_3x3, A, R; + adj << R, Z_3x3, A, R; // Gives [R 0; A R] return adj; } @@ -354,6 +354,14 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, return R_ * point + t_; } +Matrix Pose3::transformFrom(const Matrix& points) const { + if (points.rows() != 3) { + throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix."); + } + const Matrix3 R = R_.matrix(); + return (R * points).colwise() + t_; // Eigen broadcasting! +} + /* ************************************************************************* */ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, OptionalJacobian<3, 3> Hpoint) const { @@ -374,6 +382,14 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, return q; } +Matrix Pose3::transformTo(const Matrix& points) const { + if (points.rows() != 3) { + throw std::invalid_argument("Pose3:transformTo expects 3*N matrix."); + } + const Matrix3 Rt = R_.transpose(); + return Rt * (points.colwise() - t_); // Eigen broadcasting! +} + /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, OptionalJacobian<1, 3> Hpoint) const { @@ -431,7 +447,7 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { const size_t n = abPointPairs.size(); if (n < 3) { - return boost::none; // we need at least three pairs + return boost::none; // we need at least three pairs } // calculate centroids @@ -451,6 +467,18 @@ boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { return Pose3(aRb, aTb); } +boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { + if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { + throw std::invalid_argument( + "Pose3:Align expects 3*N matrices of equal shape."); + } + Point3Pairs abPointPairs; + for (size_t j=0; j < a.cols(); j++) { + abPointPairs.emplace_back(a.col(j), b.col(j)); + } + return Pose3::Align(abPointPairs); +} + boost::optional align(const Point3Pairs &baPointPairs) { Point3Pairs abPointPairs; for (const Point3Pair &baPair : baPointPairs) { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 884d0760c..c36047349 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -85,6 +85,9 @@ public: */ static boost::optional Align(const std::vector& abPointPairs); + // Version of Pose3::Align that takes 2 matrices. + static boost::optional Align(const Matrix& a, const Matrix& b); + /// @} /// @name Testable /// @{ @@ -249,6 +252,13 @@ public: Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself = boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + /** + * @brief transform many points in Pose coordinates and transform to world. + * @param points 3*N matrix in Pose coordinates + * @return points in world coordinates, as 3*N Matrix + */ + Matrix transformFrom(const Matrix& points) const; + /** syntactic sugar for transformFrom */ inline Point3 operator*(const Point3& point) const { return transformFrom(point); @@ -264,6 +274,13 @@ public: Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself = boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + /** + * @brief transform many points in world coordinates and transform to Pose. + * @param points 3*N matrix in world coordinates + * @return points in Pose coordinates, as 3*N Matrix + */ + Matrix transformTo(const Matrix& points) const; + /// @} /// @name Standard Interface /// @{ diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp index c6cff4214..7088513d5 100644 --- a/gtsam/geometry/SOn.cpp +++ b/gtsam/geometry/SOn.cpp @@ -22,7 +22,7 @@ namespace gtsam { template <> -GTSAM_EXPORT void SOn::Hat(const Vector &xi, Eigen::Ref X) { +void SOn::Hat(const Vector &xi, Eigen::Ref X) { size_t n = AmbientDim(xi.size()); if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); @@ -48,7 +48,7 @@ GTSAM_EXPORT void SOn::Hat(const Vector &xi, Eigen::Ref X) { } } -template <> GTSAM_EXPORT Matrix SOn::Hat(const Vector &xi) { +template <> Matrix SOn::Hat(const Vector &xi) { size_t n = AmbientDim(xi.size()); Matrix X(n, n); // allocate space for n*n skew-symmetric matrix SOn::Hat(xi, X); @@ -56,7 +56,6 @@ template <> GTSAM_EXPORT Matrix SOn::Hat(const Vector &xi) { } template <> -GTSAM_EXPORT Vector SOn::Vee(const Matrix& X) { const size_t n = X.rows(); if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); @@ -104,7 +103,9 @@ SOn LieGroup::between(const SOn& g, DynamicJacobian H1, } // Dynamic version of vec -template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const { +template <> +typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const +{ const size_t n = rows(), n2 = n * n; // Vectorize diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 86b6019e1..af0e7a3cf 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -24,6 +24,8 @@ #include #include +#include + #include // TODO(frank): how to avoid? #include #include @@ -356,17 +358,21 @@ Vector SOn::Vee(const Matrix& X); using DynamicJacobian = OptionalJacobian; template <> +GTSAM_EXPORT SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, DynamicJacobian H2) const; template <> +GTSAM_EXPORT SOn LieGroup::between(const SOn& g, DynamicJacobian H1, DynamicJacobian H2) const; /* * Specialize dynamic vec. */ -template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const; +template <> +GTSAM_EXPORT +typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const; /** Serialization function */ template diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 27d41a014..ebd5c63c9 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -23,11 +23,12 @@ #include #include #include +#include +#include #include #include #include -#include #include #include @@ -39,7 +40,7 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class Unit3 { +class GTSAM_EXPORT Unit3 { private: @@ -96,7 +97,7 @@ public: } /// Named constructor from Point3 with optional Jacobian - GTSAM_EXPORT static Unit3 FromPoint3(const Point3& point, // + static Unit3 FromPoint3(const Point3& point, // OptionalJacobian<2, 3> H = boost::none); /** @@ -105,7 +106,7 @@ public: * std::mt19937 engine(42); * Unit3 unit = Unit3::Random(engine); */ - GTSAM_EXPORT static Unit3 Random(std::mt19937 & rng); + static Unit3 Random(std::mt19937 & rng); /// @} @@ -115,7 +116,7 @@ public: friend std::ostream& operator<<(std::ostream& os, const Unit3& pair); /// The print fuction - GTSAM_EXPORT void print(const std::string& s = std::string()) const; + void print(const std::string& s = std::string()) const; /// The equals function with tolerance bool equals(const Unit3& s, double tol = 1e-9) const { @@ -132,16 +133,16 @@ public: * tangent to the sphere at the current direction. * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1. */ - GTSAM_EXPORT const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; + const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; /// Return skew-symmetric associated with 3D point on unit sphere - GTSAM_EXPORT Matrix3 skew() const; + Matrix3 skew() const; /// Return unit-norm Point3 - GTSAM_EXPORT Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; + Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; /// Return unit-norm Vector - GTSAM_EXPORT Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; + Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { @@ -149,20 +150,20 @@ public: } /// Return dot product with q - GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // + double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // OptionalJacobian<1,2> H2 = boost::none) const; /// Signed, vector-valued error between two directions /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. - GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; /// Signed, vector-valued error between two directions /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. - GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // + Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // OptionalJacobian<2, 2> H_q = boost::none) const; /// Distance between two directions - GTSAM_EXPORT double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; + double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; /// Cross-product between two Unit3s Unit3 cross(const Unit3& q) const { @@ -195,10 +196,10 @@ public: }; /// The retract function - GTSAM_EXPORT Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; + Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; /// The local coordinates function - GTSAM_EXPORT Vector2 localCoordinates(const Unit3& s) const; + Vector2 localCoordinates(const Unit3& s) const; /// @} diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 1e42966f8..5aeac37ec 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -406,6 +406,10 @@ class Pose2 { gtsam::Point2 transformFrom(const gtsam::Point2& p) const; gtsam::Point2 transformTo(const gtsam::Point2& p) const; + // Matrix versions + Matrix transformFrom(const Matrix& points) const; + Matrix transformTo(const Matrix& points) const; + // Standard Interface double x() const; double y() const; @@ -431,6 +435,9 @@ class Pose3 { Pose3(const gtsam::Pose2& pose2); Pose3(Matrix mat); + static boost::optional Align(const gtsam::Point3Pairs& abPointPairs); + static boost::optional Align(const gtsam::Matrix& a, const gtsam::Matrix& b); + // Testable void print(string s = "") const; bool equals(const gtsam::Pose3& pose, double tol) const; @@ -470,6 +477,10 @@ class Pose3 { gtsam::Point3 transformFrom(const gtsam::Point3& point) const; gtsam::Point3 transformTo(const gtsam::Point3& point) const; + // Matrix versions + Matrix transformFrom(const Matrix& points) const; + Matrix transformTo(const Matrix& points) const; + // Standard Interface gtsam::Rot3 rotation() const; gtsam::Point3 translation() const; diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index cd576f900..020cab2f9 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -160,7 +160,7 @@ TEST(Cal3Bundler, retract) { } /* ************************************************************************* */ -TEST(Cal3_S2, Print) { +TEST(Cal3Bundler, Print) { Cal3Bundler cal(1, 2, 3, 4, 5); std::stringstream os; os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2() diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index d17dc7689..0df858aa8 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -796,44 +796,39 @@ TEST(Pose2, align_4) { } //****************************************************************************** +namespace { +Pose2 id; Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5))); Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); +} // namespace //****************************************************************************** -TEST(Pose2 , Invariants) { - Pose2 id; - - EXPECT(check_group_invariants(id,id)); - EXPECT(check_group_invariants(id,T1)); - EXPECT(check_group_invariants(T2,id)); - EXPECT(check_group_invariants(T2,T1)); - - EXPECT(check_manifold_invariants(id,id)); - EXPECT(check_manifold_invariants(id,T1)); - EXPECT(check_manifold_invariants(T2,id)); - EXPECT(check_manifold_invariants(T2,T1)); +TEST(Pose2, Invariants) { + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T1)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T1)); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T1)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T1)); } //****************************************************************************** -TEST(Pose2 , LieGroupDerivatives) { - Pose2 id; - - CHECK_LIE_GROUP_DERIVATIVES(id,id); - CHECK_LIE_GROUP_DERIVATIVES(id,T2); - CHECK_LIE_GROUP_DERIVATIVES(T2,id); - CHECK_LIE_GROUP_DERIVATIVES(T2,T1); - +TEST(Pose2, LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T1); } //****************************************************************************** -TEST(Pose2 , ChartDerivatives) { - Pose2 id; - - CHECK_CHART_DERIVATIVES(id,id); - CHECK_CHART_DERIVATIVES(id,T2); - CHECK_CHART_DERIVATIVES(T2,id); - CHECK_CHART_DERIVATIVES(T2,T1); +TEST(Pose2, ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, T2); + CHECK_CHART_DERIVATIVES(T2, id); + CHECK_CHART_DERIVATIVES(T2, T1); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index e862b94ad..281c71f7c 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -80,12 +80,6 @@ TEST(Quaternion , Compose) { EXPECT(traits::Equals(expected, actual)); } -//****************************************************************************** -Vector3 Q_z_axis(0, 0, 1); -Q id(Eigen::AngleAxisd(0, Q_z_axis)); -Q R1(Eigen::AngleAxisd(1, Q_z_axis)); -Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); - //****************************************************************************** TEST(Quaternion , Between) { Vector3 z_axis(0, 0, 1); @@ -108,7 +102,15 @@ TEST(Quaternion , Inverse) { } //****************************************************************************** -TEST(Quaternion , Invariants) { +namespace { +Vector3 Q_z_axis(0, 0, 1); +Q id(Eigen::AngleAxisd(0, Q_z_axis)); +Q R1(Eigen::AngleAxisd(1, Q_z_axis)); +Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); +} // namespace + +//****************************************************************************** +TEST(Quaternion, Invariants) { EXPECT(check_group_invariants(id, id)); EXPECT(check_group_invariants(id, R1)); EXPECT(check_group_invariants(R2, id)); @@ -121,7 +123,7 @@ TEST(Quaternion , Invariants) { } //****************************************************************************** -TEST(Quaternion , LieGroupDerivatives) { +TEST(Quaternion, LieGroupDerivatives) { CHECK_LIE_GROUP_DERIVATIVES(id, id); CHECK_LIE_GROUP_DERIVATIVES(id, R2); CHECK_LIE_GROUP_DERIVATIVES(R2, id); @@ -129,7 +131,7 @@ TEST(Quaternion , LieGroupDerivatives) { } //****************************************************************************** -TEST(Quaternion , ChartDerivatives) { +TEST(Quaternion, ChartDerivatives) { CHECK_CHART_DERIVATIVES(id, id); CHECK_CHART_DERIVATIVES(id, R2); CHECK_CHART_DERIVATIVES(R2, id); diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 7cd27a9da..5a087edcd 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -156,44 +156,39 @@ TEST( Rot2, relativeBearing ) } //****************************************************************************** +namespace { +Rot2 id; Rot2 T1(0.1); Rot2 T2(0.2); +} // namespace //****************************************************************************** -TEST(Rot2 , Invariants) { - Rot2 id; - - EXPECT(check_group_invariants(id,id)); - EXPECT(check_group_invariants(id,T1)); - EXPECT(check_group_invariants(T2,id)); - EXPECT(check_group_invariants(T2,T1)); - - EXPECT(check_manifold_invariants(id,id)); - EXPECT(check_manifold_invariants(id,T1)); - EXPECT(check_manifold_invariants(T2,id)); - EXPECT(check_manifold_invariants(T2,T1)); +TEST(Rot2, Invariants) { + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T1)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T1)); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T1)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T1)); } //****************************************************************************** -TEST(Rot2 , LieGroupDerivatives) { - Rot2 id; - - CHECK_LIE_GROUP_DERIVATIVES(id,id); - CHECK_LIE_GROUP_DERIVATIVES(id,T2); - CHECK_LIE_GROUP_DERIVATIVES(T2,id); - CHECK_LIE_GROUP_DERIVATIVES(T2,T1); - +TEST(Rot2, LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T1); } //****************************************************************************** -TEST(Rot2 , ChartDerivatives) { - Rot2 id; - - CHECK_CHART_DERIVATIVES(id,id); - CHECK_CHART_DERIVATIVES(id,T2); - CHECK_CHART_DERIVATIVES(T2,id); - CHECK_CHART_DERIVATIVES(T2,T1); +TEST(Rot2, ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, T2); + CHECK_CHART_DERIVATIVES(T2, id); + CHECK_CHART_DERIVATIVES(T2, T1); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index dc4b888b3..1df342d57 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -640,46 +640,44 @@ TEST( Rot3, slerp) } //****************************************************************************** +namespace { +Rot3 id; Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1)); Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2)); +} // namespace //****************************************************************************** -TEST(Rot3 , Invariants) { - Rot3 id; +TEST(Rot3, Invariants) { + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T1)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T1)); + EXPECT(check_group_invariants(T1, T2)); - EXPECT(check_group_invariants(id,id)); - EXPECT(check_group_invariants(id,T1)); - EXPECT(check_group_invariants(T2,id)); - EXPECT(check_group_invariants(T2,T1)); - EXPECT(check_group_invariants(T1,T2)); - - EXPECT(check_manifold_invariants(id,id)); - EXPECT(check_manifold_invariants(id,T1)); - EXPECT(check_manifold_invariants(T2,id)); - EXPECT(check_manifold_invariants(T2,T1)); - EXPECT(check_manifold_invariants(T1,T2)); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T1)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T1)); + EXPECT(check_manifold_invariants(T1, T2)); } //****************************************************************************** -TEST(Rot3 , LieGroupDerivatives) { - Rot3 id; - - CHECK_LIE_GROUP_DERIVATIVES(id,id); - CHECK_LIE_GROUP_DERIVATIVES(id,T2); - CHECK_LIE_GROUP_DERIVATIVES(T2,id); - CHECK_LIE_GROUP_DERIVATIVES(T1,T2); - CHECK_LIE_GROUP_DERIVATIVES(T2,T1); +TEST(Rot3, LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T1, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, T1); } //****************************************************************************** -TEST(Rot3 , ChartDerivatives) { - Rot3 id; +TEST(Rot3, ChartDerivatives) { 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(T1,T2); - CHECK_CHART_DERIVATIVES(T2,T1); + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, T2); + CHECK_CHART_DERIVATIVES(T2, id); + CHECK_CHART_DERIVATIVES(T1, T2); + CHECK_CHART_DERIVATIVES(T2, T1); } } diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 910d482b0..96c1cce32 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -67,10 +67,12 @@ TEST(SO3, ClosestTo) { } //****************************************************************************** +namespace { SO3 id; Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3); SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); +} // namespace /* ************************************************************************* */ TEST(SO3, ChordalMean) { @@ -79,16 +81,16 @@ TEST(SO3, ChordalMean) { } //****************************************************************************** +// Check that Hat specialization is equal to dynamic version TEST(SO3, Hat) { - // Check that Hat specialization is equal to dynamic version EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis))); EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2))); EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3))); } //****************************************************************************** +// Check that Hat specialization is equal to dynamic version TEST(SO3, Vee) { - // Check that Hat specialization is equal to dynamic version auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1))); EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2))); diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index 5486755f7..fa550723a 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -48,6 +48,14 @@ TEST(SO4, Concept) { } //****************************************************************************** +TEST(SO4, Random) { + std::mt19937 rng(42); + auto Q = SO4::Random(rng); + EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); +} + +//****************************************************************************** +namespace { SO4 id; Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); SO4 Q1 = SO4::Expmap(v1); @@ -55,13 +63,8 @@ Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished(); SO4 Q2 = SO4::Expmap(v2); Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); SO4 Q3 = SO4::Expmap(v3); +} // namespace -//****************************************************************************** -TEST(SO4, Random) { - std::mt19937 rng(42); - auto Q = SO4::Random(rng); - EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); -} //****************************************************************************** TEST(SO4, Expmap) { // If we do exponential map in SO(3) subgroup, topleft should be equal to R1. @@ -84,16 +87,16 @@ TEST(SO4, Expmap) { } //****************************************************************************** +// Check that Hat specialization is equal to dynamic version TEST(SO4, Hat) { - // Check that Hat specialization is equal to dynamic version EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1))); EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2))); EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3))); } //****************************************************************************** +// Check that Hat specialization is equal to dynamic version TEST(SO4, Vee) { - // Check that Hat specialization is equal to dynamic version auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1))); EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2))); @@ -116,8 +119,8 @@ TEST(SO4, Retract) { } //****************************************************************************** +// Check that Cayley is identical to dynamic version TEST(SO4, Local) { - // Check that Cayley is identical to dynamic version EXPECT( assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1)))); EXPECT( @@ -166,9 +169,7 @@ TEST(SO4, vec) { Matrix actualH; const Vector16 actual = Q2.vec(actualH); EXPECT(assert_equal(expected, actual)); - std::function f = [](const SO4& Q) { - return Q.vec(); - }; + std::function f = [](const SO4& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); EXPECT(assert_equal(numericalH, actualH)); } diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 3a09f49bc..fb66fb6a2 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -38,7 +39,7 @@ using namespace boost::assign; // Some common constants static const boost::shared_ptr sharedCal = // - boost::make_shared(1500, 1200, 0, 640, 480); + boost::make_shared(1500, 1200, 0.1, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); @@ -95,11 +96,123 @@ TEST(triangulation, twoPoses) { EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } +//****************************************************************************** +// Simple test with a well-behaved two camera situation with Cal3DS2 calibration. +TEST(triangulation, twoPosesCal3DS2) { + static const boost::shared_ptr sharedDistortedCal = // + boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, + -0.0003); + + PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + + PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + + // 0. Project two landmarks into two cameras and triangulate + Point2 z1Distorted = camera1Distorted.project(landmark); + Point2 z2Distorted = camera2Distorted.project(landmark); + + vector poses; + Point2Vector measurements; + + poses += pose1, pose2; + measurements += z1Distorted, z2Distorted; + + double rank_tol = 1e-9; + + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual1, 1e-7)); + + // 2. test with optimization on, same answer + optimize = true; + boost::optional actual2 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual2, 1e-7)); + + // 3. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) + measurements.at(0) += Point2(0.1, 0.5); + measurements.at(1) += Point2(-0.2, 0.3); + optimize = false; + boost::optional actual3 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); + + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); +} + +//****************************************************************************** +// Simple test with a well-behaved two camera situation with Fisheye +// calibration. +TEST(triangulation, twoPosesFisheye) { + using Calibration = Cal3Fisheye; + static const boost::shared_ptr sharedDistortedCal = // + boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, + 0.0001, -0.0003); + + PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + + PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + + // 0. Project two landmarks into two cameras and triangulate + Point2 z1Distorted = camera1Distorted.project(landmark); + Point2 z2Distorted = camera2Distorted.project(landmark); + + vector poses; + Point2Vector measurements; + + poses += pose1, pose2; + measurements += z1Distorted, z2Distorted; + + double rank_tol = 1e-9; + + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual1, 1e-7)); + + // 2. test with optimization on, same answer + optimize = true; + boost::optional actual2 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual2, 1e-7)); + + // 3. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) + measurements.at(0) += Point2(0.1, 0.5); + measurements.at(1) += Point2(-0.2, 0.3); + optimize = false; + boost::optional actual3 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); + + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); +} + //****************************************************************************** // Similar, but now with Bundler calibration TEST(triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // - boost::make_shared(1500, 0, 0, 640, 480); + boost::make_shared(1500, 0.1, 0.2, 640, 480); PinholeCamera camera1(pose1, *bundlerCal); PinholeCamera camera2(pose2, *bundlerCal); @@ -117,7 +230,8 @@ TEST(triangulation, twoPosesBundler) { double rank_tol = 1e-9; boost::optional actual = // - triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, + optimize); EXPECT(assert_equal(landmark, *actual, 1e-7)); // Add some noise and try again @@ -125,8 +239,9 @@ TEST(triangulation, twoPosesBundler) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional actual2 = // - triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4)); + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, + optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3)); } //****************************************************************************** @@ -336,6 +451,31 @@ TEST(triangulation, fourPoses_distinct_Ks) { #endif } +//****************************************************************************** +TEST(triangulation, fourPoses_distinct_Ks_distortion) { + Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + PinholeCamera camera1(pose1, K1); + + // create second camera 1 meter to the right of first camera + Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001); + PinholeCamera camera2(pose2, K2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 z1 = camera1.project(landmark); + Point2 z2 = camera2.project(landmark); + + CameraSet> cameras; + Point2Vector measurements; + + cameras += camera1, camera2; + measurements += z1, z2; + + boost::optional actual = // + triangulatePoint3(cameras, measurements); + EXPECT(assert_equal(landmark, *actual, 1e-2)); +} + //****************************************************************************** TEST(triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index af01d3a36..49b5aef04 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -227,6 +227,109 @@ std::vector> projectionMatricesFrom return projection_matrices; } +/** Create a pinhole calibration from a different Cal3 object, removing + * distortion. + * + * @tparam CALIBRATION Original calibration object. + * @param cal Input calibration object. + * @return Cal3_S2 with only the pinhole elements of cal. + */ +template +Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) { + const auto& K = cal.K(); + return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)); +} + +/** Internal undistortMeasurement to be used by undistortMeasurement and + * undistortMeasurements */ +template +MEASUREMENT undistortMeasurementInternal( + const CALIBRATION& cal, const MEASUREMENT& measurement, + boost::optional pinholeCal = boost::none) { + if (!pinholeCal) { + pinholeCal = createPinholeCalibration(cal); + } + return pinholeCal->uncalibrate(cal.calibrate(measurement)); +} + +/** Remove distortion for measurements so as if the measurements came from a + * pinhole camera. + * + * Removes distortion but maintains the K matrix of the initial cal. Operates by + * calibrating using full calibration and uncalibrating with only the pinhole + * component of the calibration. + * @tparam CALIBRATION Calibration type to use. + * @param cal Calibration with which measurements were taken. + * @param measurements Vector of measurements to undistort. + * @return measurements with the effect of the distortion of sharedCal removed. + */ +template +Point2Vector undistortMeasurements(const CALIBRATION& cal, + const Point2Vector& measurements) { + Cal3_S2 pinholeCalibration = createPinholeCalibration(cal); + Point2Vector undistortedMeasurements; + // Calibrate with cal and uncalibrate with pinhole version of cal so that + // measurements are undistorted. + std::transform(measurements.begin(), measurements.end(), + std::back_inserter(undistortedMeasurements), + [&cal, &pinholeCalibration](const Point2& measurement) { + return undistortMeasurementInternal( + cal, measurement, pinholeCalibration); + }); + return undistortedMeasurements; +} + +/** Specialization for Cal3_S2 as it doesn't need to be undistorted. */ +template <> +inline Point2Vector undistortMeasurements(const Cal3_S2& cal, + const Point2Vector& measurements) { + return measurements; +} + +/** Remove distortion for measurements so as if the measurements came from a + * pinhole camera. + * + * Removes distortion but maintains the K matrix of the initial calibrations. + * Operates by calibrating using full calibration and uncalibrating with only + * the pinhole component of the calibration. + * @tparam CAMERA Camera type to use. + * @param cameras Cameras corresponding to each measurement. + * @param measurements Vector of measurements to undistort. + * @return measurements with the effect of the distortion of the camera removed. + */ +template +typename CAMERA::MeasurementVector undistortMeasurements( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { + const size_t num_meas = cameras.size(); + assert(num_meas == measurements.size()); + typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); + for (size_t ii = 0; ii < num_meas; ++ii) { + // Calibrate with cal and uncalibrate with pinhole version of cal so that + // measurements are undistorted. + undistortedMeasurements[ii] = + undistortMeasurementInternal( + cameras[ii].calibration(), measurements[ii]); + } + return undistortedMeasurements; +} + +/** Specialize for Cal3_S2 to do nothing. */ +template > +inline PinholeCamera::MeasurementVector undistortMeasurements( + const CameraSet>& cameras, + const PinholeCamera::MeasurementVector& measurements) { + return measurements; +} + +/** Specialize for SphericalCamera to do nothing. */ +template +inline SphericalCamera::MeasurementVector undistortMeasurements( + const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { + return measurements; +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -253,8 +356,13 @@ Point3 triangulatePoint3(const std::vector& poses, // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(*sharedCal, measurements); + // Triangulate linearly - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + Point3 point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); // Then refine using non-linear optimization if (optimize) @@ -300,7 +408,13 @@ Point3 triangulatePoint3( // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromCameras(cameras); - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(cameras, measurements); + + Point3 point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); // The n refine using non-linear optimization if (optimize) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index a32b3ce22..5b053ebee 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -139,7 +139,7 @@ namespace gtsam { return nodes_.empty(); } - /** return nodes */ + /** Return nodes. Each node is a clique of variables obtained after elimination. */ const Nodes& nodes() const { return nodes_; } /** Access node by variable */ diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 440d2b828..2ac2c0dde 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -25,11 +25,7 @@ #include #ifdef GTSAM_SUPPORT_NESTED_DISSECTION -#ifdef GTSAM_USE_SYSTEM_METIS #include -#else -#include -#endif #endif using namespace std; diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 0305218af..6fdca0d89 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -270,17 +270,7 @@ TEST(Ordering, MetisLoop) { symbolicGraph.push_factor(0, 5); // METIS -#if !defined(__APPLE__) - { - Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); - // - P( 0 4 1) - // | - P( 2 | 4 1) - // | | - P( 3 | 4 2) - // | - P( 5 | 0 1) - Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1)); - EXPECT(assert_equal(expected, actual)); - } -#else +#if defined(__APPLE__) { Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); // - P( 1 0 3) @@ -290,6 +280,26 @@ TEST(Ordering, MetisLoop) { Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); EXPECT(assert_equal(expected, actual)); } +#elif defined(_WIN32) + { + Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); + // - P( 0 5 2) + // | - P( 3 | 5 2) + // | | - P( 4 | 5 3) + // | - P( 1 | 0 2) + Ordering expected = Ordering(list_of(4)(3)(1)(0)(5)(2)); + EXPECT(assert_equal(expected, actual)); + } +#else + { + Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); + // - P( 0 4 1) + // | - P( 2 | 4 1) + // | | - P( 3 | 4 2) + // | - P( 5 | 0 1) + Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1)); + EXPECT(assert_equal(expected, actual)); + } #endif } #endif diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index cf3f78b73..c44ab246b 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -89,11 +89,15 @@ namespace gtsam { /* ************************************************************************ */ void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const { - cout << s << " Conditional density "; + cout << s << " p("; for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { - cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; + cout << (boost::format("%1%")%(formatter(*it))).str() << " "; } - cout << endl; + cout << "|"; + for (const_iterator it = beginParents(); it != endParents(); ++it) { + cout << " " << (boost::format("%1%")%(formatter(*it))).str(); + } + cout << ")" << endl; cout << formatMatrixIndented(" R = ", R()) << endl; for (const_iterator it = beginParents() ; it != endParents() ; ++it) { cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it)) diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 2f7aa312b..343396c0a 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -17,42 +17,41 @@ */ #include +#include +#include -using namespace std; +using std::cout; +using std::endl; +using std::string; namespace gtsam { - /* ************************************************************************* */ - GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, - const Vector& mean, - double sigma) { - return GaussianDensity(key, mean, - Matrix::Identity(mean.size(), mean.size()), - noiseModel::Isotropic::Sigma(mean.size(), sigma)); - } +/* ************************************************************************* */ +GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, + double sigma) { + return GaussianDensity(key, mean, Matrix::Identity(mean.size(), mean.size()), + noiseModel::Isotropic::Sigma(mean.size(), sigma)); +} - /* ************************************************************************* */ - void GaussianDensity::print(const string &s, const KeyFormatter& formatter) const - { - cout << s << ": density on "; - for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) - cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; - cout << endl; - gtsam::print(mean(), "mean: "); - gtsam::print(covariance(), "covariance: "); - if(model_) - model_->print("Noise model: "); - } +/* ************************************************************************* */ +void GaussianDensity::print(const string& s, + const KeyFormatter& formatter) const { + cout << s << ": density on "; + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) + cout << (boost::format("[%1%]") % (formatter(*it))).str() << " "; + cout << endl; + gtsam::print(mean(), "mean: "); + gtsam::print(covariance(), "covariance: "); + if (model_) model_->print("Noise model: "); +} - /* ************************************************************************* */ - Vector GaussianDensity::mean() const { - VectorValues soln = this->solve(VectorValues()); - return soln[firstFrontalKey()]; - } +/* ************************************************************************* */ +Vector GaussianDensity::mean() const { + VectorValues soln = this->solve(VectorValues()); + return soln[firstFrontalKey()]; +} - /* ************************************************************************* */ - Matrix GaussianDensity::covariance() const { - return information().inverse(); - } +/* ************************************************************************* */ +Matrix GaussianDensity::covariance() const { return information().inverse(); } -} // gtsam +} // namespace gtsam diff --git a/gtsam/linear/GaussianJunctionTree.h b/gtsam/linear/GaussianJunctionTree.h index 67e5a374b..c7f13ea5c 100644 --- a/gtsam/linear/GaussianJunctionTree.h +++ b/gtsam/linear/GaussianJunctionTree.h @@ -16,6 +16,8 @@ * @author Richard Roberts */ +#pragma once + #include #include #include diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index bf799a2ba..7307c4a68 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -19,6 +19,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index cf10cf115..8bcef6fcc 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -134,7 +134,7 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, /* ************************************************************************* */ void Gaussian::print(const string& name) const { - gtsam::print(thisR(), name + "Gaussian"); + gtsam::print(thisR(), name + "Gaussian "); } /* ************************************************************************* */ @@ -285,7 +285,7 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { /* ************************************************************************* */ void Diagonal::print(const string& name) const { - gtsam::print(sigmas_, name + "diagonal sigmas"); + gtsam::print(sigmas_, name + "diagonal sigmas "); } /* ************************************************************************* */ @@ -355,8 +355,8 @@ bool Constrained::constrained(size_t i) const { /* ************************************************************************* */ void Constrained::print(const std::string& name) const { - gtsam::print(sigmas_, name + "constrained sigmas"); - gtsam::print(mu_, name + "constrained mu"); + gtsam::print(sigmas_, name + "constrained sigmas "); + gtsam::print(mu_, name + "constrained mu "); } /* ************************************************************************* */ diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 811e07121..d19ac6de5 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -15,6 +15,8 @@ * @author Richard Roberts */ +#pragma once + #include #include #include diff --git a/gtsam/linear/tests/powerMethodExample.h b/gtsam/linear/tests/powerMethodExample.h index f80299386..994fcc640 100644 --- a/gtsam/linear/tests/powerMethodExample.h +++ b/gtsam/linear/tests/powerMethodExample.h @@ -19,6 +19,8 @@ * PowerMethod/AcceleratedPowerMethod */ +#pragma once + #include #include diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 8525cf9e0..4a9515207 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -40,6 +40,7 @@ using namespace gtsam; using namespace std; using namespace boost::assign; using symbol_shorthand::X; +using symbol_shorthand::Y; static const double tol = 1e-5; @@ -396,6 +397,45 @@ TEST(GaussianConditional, sample) { // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5)); } +/* ************************************************************************* */ +TEST(GaussianConditional, Print) { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished(); + const Vector2 b(20, 40); + const double sigma = 3; + + std::string s = "GaussianConditional"; + + auto conditional = + GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); + + // Test printing for single parent. + std::string expected = + "GaussianConditional p(x0 | x1)\n" + " R = [ 1 0 ]\n" + " [ 0 1 ]\n" + " S[x1] = [ -1 -2 ]\n" + " [ -3 -4 ]\n" + " d = [ 20 40 ]\n" + "isotropic dim=2 sigma=3\n"; + EXPECT(assert_print_equal(expected, conditional, s)); + + // Test printing for multiple parents. + auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, Y(0), A2, + Y(1), b, sigma); + std::string expected2 = + "GaussianConditional p(x0 | y0 y1)\n" + " R = [ 1 0 ]\n" + " [ 0 1 ]\n" + " S[y0] = [ -1 -2 ]\n" + " [ -3 -4 ]\n" + " S[y1] = [ -5 -6 ]\n" + " [ -7 -8 ]\n" + " d = [ 20 40 ]\n" + "isotropic dim=2 sigma=3\n"; + EXPECT(assert_print_equal(expected2, conditional2, s)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 7d086eb57..3fe2cf4d1 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -256,6 +256,3 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) { } /// namespace gtsam -/// Boost serialization export definition for derived class -BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams) - diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 89e8b574f..068a17ca4 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -351,6 +351,3 @@ template <> struct traits : public Testable {}; } // namespace gtsam - -/// Add Boost serialization export key (declaration) for derived class -BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index ed68ac077..6ab4c2f02 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -15,6 +15,8 @@ * @author Asa Hammond */ +#pragma once + #include #include diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 74e9177d5..895ac6512 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -16,6 +16,8 @@ * @date January 29, 2014 */ +#pragma once + #include #include #include diff --git a/gtsam/navigation/PreintegrationParams.cpp b/gtsam/navigation/PreintegrationParams.cpp index 2298bb696..2548f95a6 100644 --- a/gtsam/navigation/PreintegrationParams.cpp +++ b/gtsam/navigation/PreintegrationParams.cpp @@ -34,7 +34,6 @@ void PreintegrationParams::print(const string& s) const { << endl; if (omegaCoriolis && use2ndOrderCoriolis) cout << "Using 2nd-order Coriolis" << endl; - if (body_P_sensor) body_P_sensor->print(" "); cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; } diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h index 5aa83e87e..6160db2a1 100644 --- a/gtsam/navigation/tests/imuFactorTesting.h +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -28,6 +28,7 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +namespace { static const Vector3 kZero = Z_3x1; typedef imuBias::ConstantBias Bias; static const Bias kZeroBiasHat, kZeroBias; @@ -43,6 +44,7 @@ static const Vector3 kGravityAlongNavZDown(0, 0, kGravity); auto radians = [](double t) { return t * M_PI / 180; }; static const double kGyroSigma = radians(0.5) / 60; // 0.5 degree ARW static const double kAccelSigma = 0.1 / 60; // 10 cm VRW +} namespace testing { diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 9304e8412..26d793528 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -19,8 +19,6 @@ #include #include #include -#include -#include #include #include @@ -63,22 +61,6 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } -/* ************************************************************************* */ -// Export Noisemodels -// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic) - -/* ************************************************************************* */ -TEST(Rot3AttitudeFactor, Serialization) { - Unit3 nDown(0, 0, -1); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); - Rot3AttitudeFactor factor(0, nDown, model); - - EXPECT(serializationTestHelpers::equalsObj(factor)); - EXPECT(serializationTestHelpers::equalsXML(factor)); - EXPECT(serializationTestHelpers::equalsBinary(factor)); -} - /* ************************************************************************* */ TEST(Rot3AttitudeFactor, CopyAndMove) { Unit3 nDown(0, 0, -1); @@ -129,17 +111,6 @@ TEST( Pose3AttitudeFactor, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } -/* ************************************************************************* */ -TEST(Pose3AttitudeFactor, Serialization) { - Unit3 nDown(0, 0, -1); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); - Pose3AttitudeFactor factor(0, nDown, model); - - EXPECT(serializationTestHelpers::equalsObj(factor)); - EXPECT(serializationTestHelpers::equalsXML(factor)); - EXPECT(serializationTestHelpers::equalsBinary(factor)); -} - /* ************************************************************************* */ TEST(Pose3AttitudeFactor, CopyAndMove) { Unit3 nDown(0, 0, -1); diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 85447facd..e2a623710 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -31,7 +31,7 @@ using namespace std; using namespace gtsam; using namespace GeographicLib; -// ************************************************************************* +namespace { // Convert from Mag to ENU // ENU Origin is where the plane was in hold next to runway // const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; @@ -51,10 +51,11 @@ Point3 bias(10, -10, 50); Point3 scaled = scale * nM; Point3 measured = nRb.inverse() * (scale * nM) + bias; -double s(scale * nM.norm()); +double s(scale* nM.norm()); Unit3 dir(nM); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); +} // namespace using boost::none; diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp index 204c1d38f..e10409f4c 100644 --- a/gtsam/navigation/tests/testMagPoseFactor.cpp +++ b/gtsam/navigation/tests/testMagPoseFactor.cpp @@ -20,7 +20,7 @@ using namespace std::placeholders; using namespace gtsam; -// ***************************************************************************** +namespace { // Magnetic field in the nav frame (NED), with units of nT. Point3 nM(22653.29982, -1956.83010, 44202.47862); @@ -51,8 +51,9 @@ SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.25); // Make up a rotation and offset of the sensor in the body frame. Pose2 body_P2_sensor(Rot2(-0.30), Point2(1.0, -2.0)); -Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), Point3(-0.1, 0.2, 0.3)); -// ***************************************************************************** +Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), + Point3(-0.1, 0.2, 0.3)); +} // namespace // ***************************************************************************** TEST(MagPoseFactor, Constructors) { diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testSerializationNavigation.cpp similarity index 60% rename from gtsam/navigation/tests/testImuFactorSerialization.cpp rename to gtsam/navigation/tests/testSerializationNavigation.cpp index e72b1fb5b..6a2155875 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testSerializationNavigation.cpp @@ -10,17 +10,19 @@ * -------------------------------------------------------------------------- */ /** - * @file testImuFactor.cpp - * @brief Unit test for ImuFactor + * @file testSerializationNavigation.cpp + * @brief serialization tests for navigation * @author Luca Carlone * @author Frank Dellaert * @author Richard Roberts * @author Stephen Williams * @author Varun Agrawal + * @date February 2022 */ #include #include +#include #include #include @@ -30,17 +32,16 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, - "gtsam_noiseModel_Constrained") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, - "gtsam_noiseModel_Diagonal") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, - "gtsam_noiseModel_Gaussian") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, - "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") +BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal") +BOOST_CLASS_EXPORT_GUID(PreintegratedImuMeasurements, "gtsam_PreintegratedImuMeasurements") +BOOST_CLASS_EXPORT_GUID(PreintegrationCombinedParams, "gtsam_PreintegrationCombinedParams") +BOOST_CLASS_EXPORT_GUID(PreintegratedCombinedMeasurements, "gtsam_PreintegratedCombinedMeasurements") template P getPreintegratedMeasurements() { @@ -69,6 +70,7 @@ P getPreintegratedMeasurements() { return pim; } +/* ************************************************************************* */ TEST(ImuFactor, serialization) { auto pim = getPreintegratedMeasurements(); @@ -83,6 +85,7 @@ TEST(ImuFactor, serialization) { EXPECT(equalsBinary(factor)); } +/* ************************************************************************* */ TEST(ImuFactor2, serialization) { auto pim = getPreintegratedMeasurements(); @@ -93,6 +96,7 @@ TEST(ImuFactor2, serialization) { EXPECT(equalsBinary(factor)); } +/* ************************************************************************* */ TEST(CombinedImuFactor, Serialization) { auto pim = getPreintegratedMeasurements(); @@ -107,6 +111,28 @@ TEST(CombinedImuFactor, Serialization) { EXPECT(equalsBinary(factor)); } +/* ************************************************************************* */ +TEST(Rot3AttitudeFactor, Serialization) { + Unit3 nDown(0, 0, -1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); + Rot3AttitudeFactor factor(0, nDown, model); + + EXPECT(serializationTestHelpers::equalsObj(factor)); + EXPECT(serializationTestHelpers::equalsXML(factor)); + EXPECT(serializationTestHelpers::equalsBinary(factor)); +} + +/* ************************************************************************* */ +TEST(Pose3AttitudeFactor, Serialization) { + Unit3 nDown(0, 0, -1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); + Pose3AttitudeFactor factor(0, nDown, model); + + EXPECT(serializationTestHelpers::equalsObj(factor)); + EXPECT(serializationTestHelpers::equalsXML(factor)); + EXPECT(serializationTestHelpers::equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index e1f8ece8d..394b22b6b 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -56,7 +56,7 @@ namespace gtsam { * MultiplyFunctor(multiplier)); */ template -class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { +class FunctorizedFactor : public NoiseModelFactor1 { private: using Base = NoiseModelFactor1; @@ -155,7 +155,7 @@ FunctorizedFactor MakeFunctorizedFactor(Key key, const R &z, * @param T2: The second argument type for the functor. */ template -class GTSAM_EXPORT FunctorizedFactor2 : public NoiseModelFactor2 { +class FunctorizedFactor2 : public NoiseModelFactor2 { private: using Base = NoiseModelFactor2; diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3025d2468..cc3fdaf34 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -184,7 +184,8 @@ class GTSAM_EXPORT GncOptimizer { /// Compute optimal solution using graduated non-convexity. Values optimize() { NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_); - BaseOptimizer baseOptimizer(graph_initial, state_); + BaseOptimizer baseOptimizer( + graph_initial, state_, params_.baseOptimizerParams); Values result = baseOptimizer.optimize(); double mu = initializeMu(); double prev_cost = graph_initial.error(result); @@ -228,7 +229,8 @@ class GTSAM_EXPORT GncOptimizer { // variable/values update NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); - BaseOptimizer baseOptimizer_iter(graph_iter, state_); + BaseOptimizer baseOptimizer_iter( + graph_iter, state_, params_.baseOptimizerParams); result = baseOptimizer_iter.optimize(); // stopping condition diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 92c2142a7..37feee837 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -295,6 +295,17 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const ISAM2UpdateParams& updateParams, const FastList& affectedKeys, const KeySet& relinKeys); + /** + * @brief Perform an incremental update of the factor graph to return a new + * Bayes Tree with affected keys. + * + * @param updateParams Parameters for the ISAM2 update. + * @param relinKeys Keys of variables to relinearize. + * @param affectedKeys The set of keys which are affected in the update. + * @param affectedKeysSet [output] Affected and contaminated keys. + * @param orphans [output] List of orphanes cliques after elimination. + * @param result [output] The result of the incremental update step. + */ void recalculateIncremental(const ISAM2UpdateParams& updateParams, const KeySet& relinKeys, const FastList& affectedKeys, diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index b249af5c5..be91f17e2 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -175,6 +175,7 @@ struct ISAM2Result { /** Getters and Setters */ size_t getVariablesRelinearized() const { return variablesRelinearized; } size_t getVariablesReeliminated() const { return variablesReeliminated; } + FactorIndices getNewFactorsIndices() const { return newFactorsIndices; } size_t getCliques() const { return cliques; } double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); } double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); } diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index 1e2c6e395..f40443457 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -35,7 +35,7 @@ class LevenbergMarquardtOptimizer; class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { public: - /** See LevenbergMarquardtParams::lmVerbosity */ + /** See LevenbergMarquardtParams::verbosityLM */ enum VerbosityLM { SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA }; diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index efc095775..16094b67a 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -23,14 +23,14 @@ namespace gtsam { * This factor does have the ability to perform relinearization under small-angle and * linearity assumptions if a linearization point is added. */ -class LinearContainerFactor : public NonlinearFactor { +class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { protected: GaussianFactor::shared_ptr factor_; boost::optional linearizationPoint_; /** direct copy constructor */ - GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); // Some handy typedefs typedef NonlinearFactor Base; @@ -44,13 +44,13 @@ public: LinearContainerFactor() {} /** Primary constructor: store a linear factor with optional linearization point */ - GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); + LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); /** Primary constructor: store a linear factor with optional linearization point */ - GTSAM_EXPORT LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values()); + LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values()); /** Constructor from shared_ptr */ - GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); // Access @@ -59,10 +59,10 @@ public: // Testable /** print */ - GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override; /** Check if two factors are equal */ - GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const override; + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override; // NonlinearFactor @@ -74,10 +74,10 @@ public: * * @return nonlinear error if linearizationPoint present, zero otherwise */ - GTSAM_EXPORT double error(const Values& c) const override; + double error(const Values& c) const override; /** get the dimension of the factor: rows of linear factor */ - GTSAM_EXPORT size_t dim() const override; + size_t dim() const override; /** Extract the linearization point used in recalculating error */ const boost::optional& linearizationPoint() const { return linearizationPoint_; } @@ -98,17 +98,17 @@ public: * TODO: better approximation of relinearization * TODO: switchable modes for approximation technique */ - GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const override; + GaussianFactor::shared_ptr linearize(const Values& c) const override; /** * Creates an anti-factor directly */ - GTSAM_EXPORT GaussianFactor::shared_ptr negateToGaussian() const; + GaussianFactor::shared_ptr negateToGaussian() const; /** * Creates the equivalent anti-factor as another LinearContainerFactor. */ - GTSAM_EXPORT NonlinearFactor::shared_ptr negateToNonlinear() const; + NonlinearFactor::shared_ptr negateToNonlinear() const; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow @@ -140,25 +140,24 @@ public: /** * Simple checks whether this is a Jacobian or Hessian factor */ - GTSAM_EXPORT bool isJacobian() const; - GTSAM_EXPORT bool isHessian() const; + bool isJacobian() const; + bool isHessian() const; /** Casts to JacobianFactor */ - GTSAM_EXPORT boost::shared_ptr toJacobian() const; + boost::shared_ptr toJacobian() const; /** Casts to HessianFactor */ - GTSAM_EXPORT boost::shared_ptr toHessian() const; + boost::shared_ptr toHessian() const; /** * Utility function for converting linear graphs to nonlinear graphs * consisting of LinearContainerFactors. */ - GTSAM_EXPORT static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph& linear_graph, const Values& linearizationPoint = Values()); protected: - GTSAM_EXPORT void initializeLinearizationPoint(const Values& linearizationPoint); + void initializeLinearizationPoint(const Values& linearizationPoint); private: /** Serialization function */ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 38d831e15..7fafd95df 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -282,7 +282,7 @@ public: * which are objects in non-linear manifolds (Lie groups). */ template -class GTSAM_EXPORT NoiseModelFactor1: public NoiseModelFactor { +class NoiseModelFactor1: public NoiseModelFactor { public: @@ -366,7 +366,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 2 * variables. To derive from this class, implement evaluateError(). */ template -class GTSAM_EXPORT NoiseModelFactor2: public NoiseModelFactor { +class NoiseModelFactor2: public NoiseModelFactor { public: @@ -441,7 +441,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 3 * variables. To derive from this class, implement evaluateError(). */ template -class GTSAM_EXPORT NoiseModelFactor3: public NoiseModelFactor { +class NoiseModelFactor3: public NoiseModelFactor { public: @@ -518,7 +518,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 4 * variables. To derive from this class, implement evaluateError(). */ template -class GTSAM_EXPORT NoiseModelFactor4: public NoiseModelFactor { +class NoiseModelFactor4: public NoiseModelFactor { public: @@ -599,7 +599,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 5 * variables. To derive from this class, implement evaluateError(). */ template -class GTSAM_EXPORT NoiseModelFactor5: public NoiseModelFactor { +class NoiseModelFactor5: public NoiseModelFactor { public: @@ -684,7 +684,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 6 * variables. To derive from this class, implement evaluateError(). */ template -class GTSAM_EXPORT NoiseModelFactor6: public NoiseModelFactor { +class NoiseModelFactor6: public NoiseModelFactor { public: diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 95f46ab6c..1cd117437 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -17,6 +17,8 @@ * @date September 2011 */ +#pragma once + #include #include #include diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 3a9b6fb11..266aa841c 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -21,6 +21,8 @@ #include #include +#include +#include namespace gtsam { @@ -34,13 +36,14 @@ namespace gtsam { * This is fixable but expensive, and does not matter in practice as most factors will sit near * zero errors anyway. However, it means that below will only be exact for the correct measurement. */ -JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { +inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, + double delta = 1e-5) { // We will fill a vector of key/Jacobians pairs (a map would sort) std::vector > jacobians; // Get size - const Eigen::VectorXd e = factor.whitenedError(values); + const Vector e = factor.whitenedError(values); const size_t rows = e.size(); // Loop over all variables @@ -51,18 +54,18 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, const size_t cols = dX.dim(key); Matrix J = Matrix::Zero(rows, cols); for (size_t col = 0; col < cols; ++col) { - Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + Vector dx = Vector::Zero(cols); dx(col) = delta; dX[key] = dx; Values eval_values = values.retract(dX); - const Eigen::VectorXd left = factor.whitenedError(eval_values); + const Vector left = factor.whitenedError(eval_values); dx(col) = -delta; dX[key] = dx; eval_values = values.retract(dX); - const Eigen::VectorXd right = factor.whitenedError(eval_values); + const Vector right = factor.whitenedError(eval_values); J.col(col) = (left - right) * one_over_2delta; } - jacobians.push_back(std::make_pair(key, J)); + jacobians.emplace_back(key, J); } // Next step...return JacobianFactor @@ -71,15 +74,15 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, namespace internal { // CPPUnitLite-style test for linearization of a factor -bool testFactorJacobians(const std::string& name_, - const NoiseModelFactor& factor, const gtsam::Values& values, double delta, - double tolerance) { - +inline bool testFactorJacobians(const std::string& name_, + const NoiseModelFactor& factor, + const gtsam::Values& values, double delta, + double tolerance) { // Create expected value by numerical differentiation JacobianFactor expected = linearizeNumerically(factor, values, delta); // Create actual value by linearize - boost::shared_ptr actual = // + auto actual = boost::dynamic_pointer_cast(factor.linearize(values)); if (!actual) return false; @@ -89,17 +92,19 @@ bool testFactorJacobians(const std::string& name_, // if not equal, test individual jacobians: if (!equal) { for (size_t i = 0; i < actual->size(); i++) { - bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)), - (Matrix) (actual->getA(actual->begin() + i)), tolerance); + bool i_good = + assert_equal((Matrix)(expected.getA(expected.begin() + i)), + (Matrix)(actual->getA(actual->begin() + i)), tolerance); if (!i_good) { - std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl; + std::cout << "Mismatch in Jacobian " << i + 1 + << " (base 1), as shown above" << std::endl; } } } return equal; } -} +} // namespace internal /// \brief Check the Jacobians produced by a factor against finite differences. /// \param factor The factor to test. @@ -109,4 +114,4 @@ bool testFactorJacobians(const std::string& name_, #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); } -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index cee839540..75e5a5135 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -16,6 +16,8 @@ * @date April 2016 */ +#pragma once + #include "NonlinearOptimizerState.h" #include diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index da5e8b29c..326b84d16 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -25,6 +25,7 @@ namespace gtsam { #include #include #include +#include #include class GraphvizFormatting : gtsam::DotWriter { @@ -228,6 +229,21 @@ class Values { void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); + void insert(size_t j, const gtsam::ParameterMatrix<1>& X); + void insert(size_t j, const gtsam::ParameterMatrix<2>& X); + void insert(size_t j, const gtsam::ParameterMatrix<3>& X); + void insert(size_t j, const gtsam::ParameterMatrix<4>& X); + void insert(size_t j, const gtsam::ParameterMatrix<5>& X); + void insert(size_t j, const gtsam::ParameterMatrix<6>& X); + void insert(size_t j, const gtsam::ParameterMatrix<7>& X); + void insert(size_t j, const gtsam::ParameterMatrix<8>& X); + void insert(size_t j, const gtsam::ParameterMatrix<9>& X); + void insert(size_t j, const gtsam::ParameterMatrix<10>& X); + void insert(size_t j, const gtsam::ParameterMatrix<11>& X); + void insert(size_t j, const gtsam::ParameterMatrix<12>& X); + void insert(size_t j, const gtsam::ParameterMatrix<13>& X); + void insert(size_t j, const gtsam::ParameterMatrix<14>& X); + void insert(size_t j, const gtsam::ParameterMatrix<15>& X); void update(size_t j, const gtsam::Point2& point2); void update(size_t j, const gtsam::Point3& point3); @@ -254,6 +270,21 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); void update(size_t j, double c); + void update(size_t j, const gtsam::ParameterMatrix<1>& X); + void update(size_t j, const gtsam::ParameterMatrix<2>& X); + void update(size_t j, const gtsam::ParameterMatrix<3>& X); + void update(size_t j, const gtsam::ParameterMatrix<4>& X); + void update(size_t j, const gtsam::ParameterMatrix<5>& X); + void update(size_t j, const gtsam::ParameterMatrix<6>& X); + void update(size_t j, const gtsam::ParameterMatrix<7>& X); + void update(size_t j, const gtsam::ParameterMatrix<8>& X); + void update(size_t j, const gtsam::ParameterMatrix<9>& X); + void update(size_t j, const gtsam::ParameterMatrix<10>& X); + void update(size_t j, const gtsam::ParameterMatrix<11>& X); + void update(size_t j, const gtsam::ParameterMatrix<12>& X); + void update(size_t j, const gtsam::ParameterMatrix<13>& X); + void update(size_t j, const gtsam::ParameterMatrix<14>& X); + void update(size_t j, const gtsam::ParameterMatrix<15>& X); void insert_or_assign(size_t j, const gtsam::Point2& point2); void insert_or_assign(size_t j, const gtsam::Point3& point3); @@ -280,6 +311,21 @@ class Values { void insert_or_assign(size_t j, Vector vector); void insert_or_assign(size_t j, Matrix matrix); void insert_or_assign(size_t j, double c); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X); template + double, + gtsam::ParameterMatrix<1>, + gtsam::ParameterMatrix<2>, + gtsam::ParameterMatrix<3>, + gtsam::ParameterMatrix<4>, + gtsam::ParameterMatrix<5>, + gtsam::ParameterMatrix<6>, + gtsam::ParameterMatrix<7>, + gtsam::ParameterMatrix<8>, + gtsam::ParameterMatrix<9>, + gtsam::ParameterMatrix<10>, + gtsam::ParameterMatrix<11>, + gtsam::ParameterMatrix<12>, + gtsam::ParameterMatrix<13>, + gtsam::ParameterMatrix<14>, + gtsam::ParameterMatrix<15>}> T at(size_t j); }; @@ -621,6 +682,7 @@ class ISAM2Result { /** Getters and Setters for all properties */ size_t getVariablesRelinearized() const; size_t getVariablesReeliminated() const; + FactorIndices getNewFactorsIndices() const; size_t getCliques() const; double getErrorBefore() const; double getErrorAfter() const; diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 5d0d5d5f2..419172f74 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -153,7 +153,7 @@ TEST(CallRecord, virtualReverseAdDispatching) { } { const int Rows = 6; - record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix::Zero(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); @@ -168,4 +168,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index 14a14fc19..214c5efa7 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -17,16 +17,14 @@ * @brief unit tests for FunctorizedFactor class */ -#include -#include -#include -#include -#include -#include -#include #include #include #include +#include +#include +#include + +#include using namespace std; using namespace gtsam; @@ -272,135 +270,6 @@ TEST(FunctorizedFactor, Lambda2) { EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); } -const size_t N = 2; - -//****************************************************************************** -TEST(FunctorizedFactor, Print2) { - const size_t M = 1; - - Vector measured = Vector::Ones(M) * 42; - - auto model = noiseModel::Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor priorFactor(key, measured, model, N, 0); - - string expected = - " keys = { X0 }\n" - " noise model: unit (1) \n" - "FunctorizedFactor(X0)\n" - " measurement: [\n" - " 42\n" - "]\n" - " noise model sigmas: 1\n"; - - EXPECT(assert_print_equal(expected, priorFactor)); -} - -//****************************************************************************** -TEST(FunctorizedFactor, VectorEvaluationFactor) { - const size_t M = 4; - - Vector measured = Vector::Zero(M); - - auto model = noiseModel::Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor priorFactor(key, measured, model, N, 0); - - NonlinearFactorGraph graph; - graph.add(priorFactor); - - ParameterMatrix stateMatrix(N); - - Values initial; - initial.insert>(key, stateMatrix); - - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; - parameters.setMaxIterations(20); - Values result = - LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); - - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); -} - -//****************************************************************************** -TEST(FunctorizedFactor, VectorComponentFactor) { - const int P = 4; - const size_t i = 2; - const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; - auto model = noiseModel::Isotropic::Sigma(1, 1.0); - VectorComponentFactor controlPrior(key, measured, model, N, i, - t, a, b); - - NonlinearFactorGraph graph; - graph.add(controlPrior); - - ParameterMatrix

stateMatrix(N); - - Values initial; - initial.insert>(key, stateMatrix); - - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; - parameters.setMaxIterations(20); - Values result = - LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); - - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); -} - -//****************************************************************************** -TEST(FunctorizedFactor, VecDerivativePrior) { - const size_t M = 4; - - Vector measured = Vector::Zero(M); - auto model = noiseModel::Isotropic::Sigma(M, 1.0); - VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); - - NonlinearFactorGraph graph; - graph.add(vecDPrior); - - ParameterMatrix stateMatrix(N); - - Values initial; - initial.insert>(key, stateMatrix); - - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; - parameters.setMaxIterations(20); - Values result = - LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); - - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); -} - -//****************************************************************************** -TEST(FunctorizedFactor, ComponentDerivativeFactor) { - const size_t M = 4; - - double measured = 0; - auto model = noiseModel::Isotropic::Sigma(1, 1.0); - ComponentDerivativeFactor controlDPrior(key, measured, model, - N, 0, 0); - - NonlinearFactorGraph graph; - graph.add(controlDPrior); - - Values initial; - ParameterMatrix stateMatrix(N); - initial.insert>(key, stateMatrix); - - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; - parameters.setMaxIterations(20); - Values result = - LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); - - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i index 7ba401f1e..90c319ede 100644 --- a/gtsam/sam/sam.i +++ b/gtsam/sam/sam.i @@ -20,6 +20,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; + + const double measured() const; }; // between points: @@ -54,6 +56,9 @@ virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; + + // Use `double` instead of template since that is all we need. + const double measured() const; }; typedef gtsam::RangeFactorWithTransform @@ -73,6 +78,8 @@ virtual class BearingFactor : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; + + const BEARING& measured() const; }; typedef gtsam::BearingFactor diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp index 17a049a1d..904bdba31 100644 --- a/gtsam/sam/tests/testBearingFactor.cpp +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -21,14 +21,13 @@ #include #include #include -#include -#include #include using namespace std; using namespace gtsam; +namespace { Key poseKey(1); Key pointKey(2); @@ -41,43 +40,18 @@ typedef BearingFactor BearingFactor3D; Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values! static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5)); BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D); - -/* ************************************************************************* */ -// Export Noisemodels -// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); - -/* ************************************************************************* */ -TEST(BearingFactor, Serialization2D) { - EXPECT(serializationTestHelpers::equalsObj(factor2D)); - EXPECT(serializationTestHelpers::equalsXML(factor2D)); - EXPECT(serializationTestHelpers::equalsBinary(factor2D)); } /* ************************************************************************* */ TEST(BearingFactor, 2D) { - // Serialize the factor - std::string serialized = serializeXML(factor2D); - - // And de-serialize it - BearingFactor2D factor; - deserializeXML(serialized, factor); - // Set the linearization point Values values; values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(pointKey, Point2(-4.0, 11.0)); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}), values, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); -} - -/* ************************************************************************* */ -TEST(BearingFactor, Serialization3D) { - EXPECT(serializationTestHelpers::equalsObj(factor3D)); - EXPECT(serializationTestHelpers::equalsXML(factor3D)); - EXPECT(serializationTestHelpers::equalsBinary(factor3D)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5); } /* ************************************************************************* */ diff --git a/gtsam/sam/tests/testBearingRangeFactor.cpp b/gtsam/sam/tests/testBearingRangeFactor.cpp index 735358d89..0dcc227c7 100644 --- a/gtsam/sam/tests/testBearingRangeFactor.cpp +++ b/gtsam/sam/tests/testBearingRangeFactor.cpp @@ -21,14 +21,13 @@ #include #include #include -#include -#include #include using namespace std; using namespace gtsam; +namespace { Key poseKey(1); Key pointKey(2); @@ -40,43 +39,18 @@ typedef BearingRangeFactor BearingRangeFactor3D; static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); BearingRangeFactor3D factor3D(poseKey, pointKey, Pose3().bearing(Point3(1, 0, 0)), 1, model3D); - -/* ************************************************************************* */ -// Export Noisemodels -// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); - -/* ************************************************************************* */ -TEST(BearingRangeFactor, Serialization2D) { - EXPECT(serializationTestHelpers::equalsObj(factor2D)); - EXPECT(serializationTestHelpers::equalsXML(factor2D)); - EXPECT(serializationTestHelpers::equalsBinary(factor2D)); } /* ************************************************************************* */ TEST(BearingRangeFactor, 2D) { - // Serialize the factor - std::string serialized = serializeXML(factor2D); - - // And de-serialize it - BearingRangeFactor2D factor; - deserializeXML(serialized, factor); - // Set the linearization point Values values; values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(pointKey, Point2(-4.0, 11.0)); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}), values, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); -} - -/* ************************************************************************* */ -TEST(BearingRangeFactor, Serialization3D) { - EXPECT(serializationTestHelpers::equalsObj(factor3D)); - EXPECT(serializationTestHelpers::equalsXML(factor3D)); - EXPECT(serializationTestHelpers::equalsBinary(factor3D)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5); } /* ************************************************************************* */ diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 5f5d4f4dd..200e1236a 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -32,42 +31,40 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; -// Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(1)); - typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform RangeFactorWithTransform3D; // Keys are deliberately *not* in sorted order to test that case. +namespace { +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(1)); + constexpr Key poseKey(2); constexpr Key pointKey(1); constexpr double measurement(10.0); -/* ************************************************************************* */ Vector factorError2D(const Pose2& pose, const Point2& point, - const RangeFactor2D& factor) { + const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } -/* ************************************************************************* */ Vector factorError3D(const Pose3& pose, const Point3& point, - const RangeFactor3D& factor) { + const RangeFactor3D& factor) { return factor.evaluateError(pose, point); } -/* ************************************************************************* */ Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, - const RangeFactorWithTransform2D& factor) { + const RangeFactorWithTransform2D& factor) { return factor.evaluateError(pose, point); } -/* ************************************************************************* */ Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, - const RangeFactorWithTransform3D& factor) { + const RangeFactorWithTransform3D& factor) { return factor.evaluateError(pose, point); } +} // namespace /* ************************************************************************* */ TEST( RangeFactor, Constructor) { @@ -75,27 +72,6 @@ TEST( RangeFactor, Constructor) { RangeFactor3D factor3D(poseKey, pointKey, measurement, model); } -/* ************************************************************************* */ -// Export Noisemodels -// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit); - -/* ************************************************************************* */ -TEST(RangeFactor, Serialization2D) { - RangeFactor2D factor2D(poseKey, pointKey, measurement, model); - EXPECT(serializationTestHelpers::equalsObj(factor2D)); - EXPECT(serializationTestHelpers::equalsXML(factor2D)); - EXPECT(serializationTestHelpers::equalsBinary(factor2D)); -} - -/* ************************************************************************* */ -TEST(RangeFactor, Serialization3D) { - RangeFactor3D factor3D(poseKey, pointKey, measurement, model); - EXPECT(serializationTestHelpers::equalsObj(factor3D)); - EXPECT(serializationTestHelpers::equalsXML(factor3D)); - EXPECT(serializationTestHelpers::equalsBinary(factor3D)); -} - /* ************************************************************************* */ TEST( RangeFactor, ConstructorWithTransform) { Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); @@ -142,28 +118,6 @@ TEST( RangeFactor, EqualsWithTransform ) { body_P_sensor_3D); CHECK(assert_equal(factor3D_1, factor3D_2)); } -/* ************************************************************************* */ -TEST( RangeFactor, EqualsAfterDeserializing) { - // Check that the same results are obtained after deserializing: - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); - - RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model, - body_P_sensor_3D), factor3D_2; - - // check with Equal() trait: - gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2); - CHECK(assert_equal(factor3D_1, factor3D_2)); - - const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0)); - const Point3 point(-2.0, 11.0, 1.0); - const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}; - - const Vector error_1 = factor3D_1.unwhitenedError(values); - const Vector error_2 = factor3D_2.unwhitenedError(values); - CHECK(assert_equal(error_1, error_2)); -} - /* ************************************************************************* */ TEST( RangeFactor, Error2D ) { // Create a factor @@ -411,7 +365,7 @@ TEST( RangeFactor, Camera) { /* ************************************************************************* */ // Do a test with non GTSAM types -namespace gtsam{ +namespace gtsam { template <> struct Range { typedef double result_type; @@ -421,7 +375,7 @@ struct Range { // derivatives not implemented } }; -} +} // namespace gtsam TEST(RangeFactor, NonGTSAM) { // Create a factor diff --git a/gtsam/sam/tests/testSerializationSam.cpp b/gtsam/sam/tests/testSerializationSam.cpp new file mode 100644 index 000000000..8fdd8f37e --- /dev/null +++ b/gtsam/sam/tests/testSerializationSam.cpp @@ -0,0 +1,140 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationSam.cpp + * @brief All serialization test in this directory + * @author Frank Dellaert + * @date February 2022 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +namespace { +Key poseKey(1); +Key pointKey(2); +constexpr double rangeMmeasurement(10.0); +} // namespace + +/* ************************************************************************* */ +// Export Noisemodels +// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html +BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); +BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit); + +/* ************************************************************************* */ +TEST(SerializationSam, BearingFactor2D) { + using BearingFactor2D = BearingFactor; + double measurement2D(10.0); + static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5)); + BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D); + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(SerializationSam, BearingFactor3D) { + using BearingFactor3D = BearingFactor; + Unit3 measurement3D = + Pose3().bearing(Point3(1, 0, 0)); // has to match values! + static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5)); + BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D); + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + +/* ************************************************************************* */ +namespace { +static SharedNoiseModel rangeNoiseModel(noiseModel::Unit::Create(1)); +} + +TEST(SerializationSam, RangeFactor2D) { + using RangeFactor2D = RangeFactor; + RangeFactor2D factor2D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel); + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(SerializationSam, RangeFactor3D) { + using RangeFactor3D = RangeFactor; + RangeFactor3D factor3D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel); + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + +/* ************************************************************************* */ +TEST(RangeFactor, EqualsAfterDeserializing) { + // Check that the same results are obtained after deserializing: + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform factor3D_1( + poseKey, pointKey, rangeMmeasurement, rangeNoiseModel, body_P_sensor_3D), + factor3D_2; + + // check with Equal() trait: + gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2); + CHECK(assert_equal(factor3D_1, factor3D_2)); + + const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0)); + const Point3 point(-2.0, 11.0, 1.0); + const Values values = {{poseKey, genericValue(pose)}, + {pointKey, genericValue(point)}}; + + const Vector error_1 = factor3D_1.unwhitenedError(values); + const Vector error_2 = factor3D_2.unwhitenedError(values); + CHECK(assert_equal(error_1, error_2)); +} + +/* ************************************************************************* */ +TEST(BearingRangeFactor, Serialization2D) { + using BearingRangeFactor2D = BearingRangeFactor; + static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5)); + BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D); + + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(BearingRangeFactor, Serialization3D) { + using BearingRangeFactor3D = BearingRangeFactor; + static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); + BearingRangeFactor3D factor3D(poseKey, pointKey, + Pose3().bearing(Point3(1, 0, 0)), 1, model3D); + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 43b1f5911..6c2676e48 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -426,7 +426,6 @@ NonlinearFactorGraph SfmData::generalSfmFactors( NonlinearFactorGraph SfmData::sfmFactorGraph( const SharedNoiseModel &model, boost::optional fixedCamera, boost::optional fixedPoint) const { - using ProjectionFactor = GeneralSFMFactor; NonlinearFactorGraph graph = generalSfmFactors(model); using noiseModel::Constrained; if (fixedCamera) { diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index a5c31534c..e035da4c7 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -300,6 +300,7 @@ class GTSAM_EXPORT ShonanAveraging { /** * Create initial Values of type SO(p) * @param p the dimensionality of the rotation manifold + * @param rng random number generator */ Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const; diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index f38c14ba7..2e81c2d56 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -35,6 +35,9 @@ using namespace gtsam; using namespace std; +// In Wrappers we have no access to this so have a default ready. +static std::mt19937 kRandomNumberGenerator(42); + TranslationRecovery::TranslationRecovery( const TranslationRecovery::TranslationEdges &relativeTranslations, const LevenbergMarquardtParams &lmParams) @@ -88,13 +91,15 @@ void TranslationRecovery::addPrior( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } -Values TranslationRecovery::initalizeRandomly() const { +Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { + uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; - auto insert = [&initial](Key j) { + auto insert = [&](Key j) { if (!initial.exists(j)) { - initial.insert(j, Vector3::Random()); + initial.insert( + j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); } }; @@ -115,10 +120,14 @@ Values TranslationRecovery::initalizeRandomly() const { return initial; } +Values TranslationRecovery::initializeRandomly() const { + return initializeRandomly(&kRandomNumberGenerator); +} + Values TranslationRecovery::run(const double scale) const { auto graph = buildGraph(); addPrior(scale, &graph); - const Values initial = initalizeRandomly(); + const Values initial = initializeRandomly(); LevenbergMarquardtOptimizer lm(graph, initial, params_); Values result = lm.optimize(); diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index c99836853..30c9a14e3 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -16,16 +16,16 @@ * @brief Recovering translations in an epipolar graph when rotations are given. */ -#include -#include -#include -#include - #include #include #include #include +#include +#include +#include +#include + namespace gtsam { // Set up an optimization problem for the unknown translations Ti in the world @@ -100,9 +100,17 @@ class TranslationRecovery { /** * @brief Create random initial translations. * + * @param rng random number generator * @return Values */ - Values initalizeRandomly() const; + Values initializeRandomly(std::mt19937 *rng) const; + + /** + * @brief Version of initializeRandomly with a fixed seed. + * + * @return Values + */ + Values initializeRandomly() const; /** * @brief Build and optimize factor graph. diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 8a1ffdd72..f80462847 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -80,7 +80,9 @@ namespace gtsam { /// @{ /// print with optional string - void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 4a60c8ba0..05e23ce6d 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -48,7 +48,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n, * element of SO(3) or SO(4). */ template -class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1 { +class FrobeniusPrior : public NoiseModelFactor1 { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; using MatrixNN = typename Rot::MatrixNN; Eigen::Matrix vecM_; ///< vectorized matrix to approximate @@ -75,7 +75,7 @@ class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1 { * The template argument can be any fixed-size SO. */ template -class GTSAM_EXPORT FrobeniusFactor : public NoiseModelFactor2 { +class FrobeniusFactor : public NoiseModelFactor2 { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; public: @@ -101,7 +101,7 @@ class GTSAM_EXPORT FrobeniusFactor : public NoiseModelFactor2 { * and in fact only SO3 and SO4 really work, as we need SO::AdjointMap. */ template -class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2 { +class FrobeniusBetweenFactor : public NoiseModelFactor2 { Rot R12_; ///< measured rotation between R1 and R2 Eigen::Matrix R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index d7b836dec..81bb790de 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -15,7 +15,7 @@ namespace gtsam { /** * Factor to measure a planar landmark from a given pose */ -class OrientedPlane3Factor: public NoiseModelFactor2 { +class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2 { protected: OrientedPlane3 measured_p_; typedef NoiseModelFactor2 Base; @@ -49,7 +49,7 @@ class OrientedPlane3Factor: public NoiseModelFactor2 { }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior -class OrientedPlane3DirectionPrior : public NoiseModelFactor1 { +class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactor1 { protected: OrientedPlane3 measured_p_; /// measured plane parameters typedef NoiseModelFactor1 Base; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 16712c429..ca158cc1d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -47,7 +47,7 @@ namespace gtsam { * @tparam CAMERA should behave like a PinholeCamera. */ template -class GTSAM_EXPORT SmartFactorBase: public NonlinearFactor { +class SmartFactorBase: public NonlinearFactor { private: typedef NonlinearFactor Base; diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 3cd69c46f..f4c0c73aa 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -42,7 +42,7 @@ namespace gtsam { * @addtogroup SLAM */ template -class GTSAM_EXPORT SmartProjectionPoseFactor +class SmartProjectionPoseFactor : public SmartProjectionFactor > { private: typedef PinholePose Camera; diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 40e9538e2..b6da02d55 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -15,6 +15,8 @@ * @author Frank Dellaert */ +#pragma once + #include #include #include diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index a2b96efab..71dd64dbb 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -177,8 +177,8 @@ boost::optional parseVertexPose(istream &is, const string &tag) { } template <> -std::map parseVariables(const std::string &filename, - size_t maxIndex) { +GTSAM_EXPORT std::map parseVariables( + const std::string &filename, size_t maxIndex) { return parseToMap(filename, parseVertexPose, maxIndex); } @@ -199,8 +199,8 @@ boost::optional parseVertexLandmark(istream &is, } template <> -std::map parseVariables(const std::string &filename, - size_t maxIndex) { +GTSAM_EXPORT std::map parseVariables( + const std::string &filename, size_t maxIndex) { return parseToMap(filename, parseVertexLandmark, maxIndex); } @@ -384,6 +384,7 @@ boost::shared_ptr createSampler(const SharedNoiseModel &model) { /* ************************************************************************* */ // Implementation of parseMeasurements for Pose2 template <> +GTSAM_EXPORT std::vector> parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, @@ -411,6 +412,7 @@ static BinaryMeasurement convert(const BinaryMeasurement &p) { } template <> +GTSAM_EXPORT std::vector> parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, @@ -426,6 +428,7 @@ parseMeasurements(const std::string &filename, /* ************************************************************************* */ // Implementation of parseFactors for Pose2 template <> +GTSAM_EXPORT std::vector::shared_ptr> parseFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, @@ -775,8 +778,8 @@ boost::optional> parseVertexPose3(istream &is, } template <> -std::map parseVariables(const std::string &filename, - size_t maxIndex) { +GTSAM_EXPORT std::map parseVariables( + const std::string &filename, size_t maxIndex) { return parseToMap(filename, parseVertexPose3, maxIndex); } @@ -793,8 +796,8 @@ boost::optional> parseVertexPoint3(istream &is, } template <> -std::map parseVariables(const std::string &filename, - size_t maxIndex) { +GTSAM_EXPORT std::map parseVariables( + const std::string &filename, size_t maxIndex) { return parseToMap(filename, parseVertexPoint3, maxIndex); } @@ -868,6 +871,7 @@ template <> struct ParseMeasurement { /* ************************************************************************* */ // Implementation of parseMeasurements for Pose3 template <> +GTSAM_EXPORT std::vector> parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, @@ -895,6 +899,7 @@ static BinaryMeasurement convert(const BinaryMeasurement &p) { } template <> +GTSAM_EXPORT std::vector> parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, @@ -910,6 +915,7 @@ parseMeasurements(const std::string &filename, /* ************************************************************************* */ // Implementation of parseFactors for Pose3 template <> +GTSAM_EXPORT std::vector::shared_ptr> parseFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, diff --git a/gtsam/slam/tests/PinholeFactor.h b/gtsam/slam/tests/PinholeFactor.h new file mode 100644 index 000000000..35500ca35 --- /dev/null +++ b/gtsam/slam/tests/PinholeFactor.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * 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 PinholeFactor.h + * @brief helper class for tests + * @author Frank Dellaert + * @date February 2022 + */ + +#pragma once + +namespace gtsam { +template +struct traits; +} + +#include +#include +#include +#include + +namespace gtsam { + +class PinholeFactor : public SmartFactorBase > { + public: + typedef SmartFactorBase > Base; + PinholeFactor() {} + PinholeFactor(const SharedNoiseModel& sharedNoiseModel, + boost::optional body_P_sensor = boost::none, + size_t expectedNumberCameras = 10) + : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} + double error(const Values& values) const override { return 0.0; } + boost::shared_ptr linearize( + const Values& values) const override { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 66be08c67..eff942799 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -29,6 +29,7 @@ using namespace std; using namespace gtsam; +namespace { // three landmarks ~5 meters infront of camera Point3 landmark1(5, 0.5, 1.2); Point3 landmark2(5, -0.5, 1.2); @@ -48,23 +49,24 @@ static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static double fov = 60; // degrees static size_t w = 640, h = 480; +} /* ************************************************************************* */ // default Cal3_S2 cameras namespace vanilla { typedef PinholeCamera Camera; typedef SmartProjectionFactor SmartFactor; -static Cal3_S2 K(fov, w, h); -static Cal3_S2 K2(1500, 1200, 0, w, h); -Camera level_camera(level_pose, K2); -Camera level_camera_right(pose_right, K2); -Point2 level_uv = level_camera.project(landmark1); -Point2 level_uv_right = level_camera_right.project(landmark1); -Camera cam1(level_pose, K2); -Camera cam2(pose_right, K2); -Camera cam3(pose_above, K2); +static const Cal3_S2 K(fov, w, h); +static const Cal3_S2 K2(1500, 1200, 0, w, h); +static const Camera level_camera(level_pose, K2); +static const Camera level_camera_right(pose_right, K2); +static const Point2 level_uv = level_camera.project(landmark1); +static const Point2 level_uv_right = level_camera_right.project(landmark1); +static const Camera cam1(level_pose, K2); +static const Camera cam2(pose_right, K2); +static const Camera cam3(pose_above, K2); typedef GeneralSFMFactor SFMFactor; -SmartProjectionParams params; +static const SmartProjectionParams params; } // namespace vanilla /* ************************************************************************* */ @@ -74,12 +76,12 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); -Camera level_camera(level_pose, sharedK); -Camera level_camera_right(pose_right, sharedK); -Camera cam1(level_pose, sharedK); -Camera cam2(pose_right, sharedK); -Camera cam3(pose_above, sharedK); +static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +static const Camera level_camera(level_pose, sharedK); +static const Camera level_camera_right(pose_right, sharedK); +static const Camera cam1(level_pose, sharedK); +static const Camera cam2(pose_right, sharedK); +static const Camera cam3(pose_above, sharedK); } // namespace vanillaPose /* ************************************************************************* */ @@ -89,12 +91,12 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); -Camera level_camera(level_pose, sharedK2); -Camera level_camera_right(pose_right, sharedK2); -Camera cam1(level_pose, sharedK2); -Camera cam2(pose_right, sharedK2); -Camera cam3(pose_above, sharedK2); +static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); +static const Camera level_camera(level_pose, sharedK2); +static const Camera level_camera_right(pose_right, sharedK2); +static const Camera cam1(level_pose, sharedK2); +static const Camera cam2(pose_right, sharedK2); +static const Camera cam3(pose_above, sharedK2); } // namespace vanillaPose2 /* *************************************************************************/ @@ -103,15 +105,15 @@ namespace bundler { typedef PinholeCamera Camera; typedef CameraSet Cameras; typedef SmartProjectionFactor SmartFactor; -static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); -Camera level_camera(level_pose, K); -Camera level_camera_right(pose_right, K); -Point2 level_uv = level_camera.project(landmark1); -Point2 level_uv_right = level_camera_right.project(landmark1); -Pose3 pose1 = level_pose; -Camera cam1(level_pose, K); -Camera cam2(pose_right, K); -Camera cam3(pose_above, K); +static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); +static const Camera level_camera(level_pose, K); +static const Camera level_camera_right(pose_right, K); +static const Point2 level_uv = level_camera.project(landmark1); +static const Point2 level_uv_right = level_camera_right.project(landmark1); +static const Pose3 pose1 = level_pose; +static const Camera cam1(level_pose, K); +static const Camera cam2(pose_right, K); +static const Camera cam3(pose_above, K); typedef GeneralSFMFactor SFMFactor; } // namespace bundler @@ -122,14 +124,14 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static boost::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, +static const boost::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); -Camera level_camera(level_pose, sharedBundlerK); -Camera level_camera_right(pose_right, sharedBundlerK); -Camera cam1(level_pose, sharedBundlerK); -Camera cam2(pose_right, sharedBundlerK); -Camera cam3(pose_above, sharedBundlerK); +static const Camera level_camera(level_pose, sharedBundlerK); +static const Camera level_camera_right(pose_right, sharedBundlerK); +static const Camera cam1(level_pose, sharedBundlerK); +static const Camera cam2(pose_right, sharedBundlerK); +static const Camera cam3(pose_above, sharedBundlerK); } // namespace bundlerPose /* ************************************************************************* */ @@ -138,12 +140,12 @@ namespace sphericalCamera { typedef SphericalCamera Camera; typedef CameraSet Cameras; typedef SmartProjectionRigFactor SmartFactorP; -static EmptyCal::shared_ptr emptyK(new EmptyCal()); -Camera level_camera(level_pose); -Camera level_camera_right(pose_right); -Camera cam1(level_pose); -Camera cam2(pose_right); -Camera cam3(pose_above); +static const EmptyCal::shared_ptr emptyK(new EmptyCal()); +static const Camera level_camera(level_pose); +static const Camera level_camera_right(pose_right); +static const Camera cam1(level_pose); +static const Camera cam2(pose_right); +static const Camera cam3(pose_above); } // namespace sphericalCamera /* *************************************************************************/ diff --git a/gtsam/slam/tests/testSerializationInSlam.cpp b/gtsam/slam/tests/testSerializationInSlam.cpp new file mode 100644 index 000000000..6aec8ecb0 --- /dev/null +++ b/gtsam/slam/tests/testSerializationInSlam.cpp @@ -0,0 +1,105 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationSlam.cpp + * @brief all serialization tests in this directory + * @author Frank Dellaert + * @date February 2022 + */ + +#include "smartFactorScenarios.h" +#include "PinholeFactor.h" + +#include +#include +#include + +#include + +#include +#include + +namespace { +static const double rankTol = 1.0; +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); +} // namespace + +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal") + +/* ************************************************************************* */ +TEST(SmartFactorBase, serialize) { + using namespace serializationTestHelpers; + PinholeFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +TEST(SerializationSlam, SmartProjectionFactor) { + using namespace vanilla; + using namespace serializationTestHelpers; + SmartFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +TEST(SerializationSlam, SmartProjectionPoseFactor) { + using namespace vanillaPose; + using namespace serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor(model, sharedK, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +TEST(SerializationSlam, SmartProjectionPoseFactor2) { + using namespace vanillaPose; + using namespace serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + Pose3 bts; + SmartFactor factor(model, sharedK, bts, params); + + // insert some measurments + KeyVector key_view; + Point2Vector meas_view; + key_view.push_back(Symbol('x', 1)); + meas_view.push_back(Point2(10, 10)); + factor.add(meas_view, key_view); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index ba46dce8d..544fd3264 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -16,47 +16,29 @@ * @date Feb 2015 */ -#include -#include -#include #include +#include +#include +#include +#include +#include -using namespace std; +#include "PinholeFactor.h" + +namespace { using namespace gtsam; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static SharedNoiseModel unit3(noiseModel::Unit::Create(3)); +} // namespace -/* ************************************************************************* */ -#include -#include +using namespace std; namespace gtsam { -class PinholeFactor: public SmartFactorBase > { -public: - typedef SmartFactorBase > Base; - PinholeFactor() {} - PinholeFactor(const SharedNoiseModel& sharedNoiseModel, - boost::optional body_P_sensor = boost::none, - size_t expectedNumberCameras = 10) - : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} - double error(const Values& values) const override { return 0.0; } - boost::shared_ptr linearize( - const Values& values) const override { - return boost::shared_ptr(new JacobianFactor()); - } -}; - -/// traits -template<> -struct traits : public Testable {}; -} - TEST(SmartFactorBase, Pinhole) { - PinholeFactor f= PinholeFactor(unit2); - f.add(Point2(0,0), 1); - f.add(Point2(0,0), 2); + PinholeFactor f = PinholeFactor(unit2); + f.add(Point2(0, 0), 1); + f.add(Point2(0, 0), 2); EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } @@ -71,7 +53,7 @@ TEST(SmartFactorBase, PinholeWithSensor) { // Camera coordinates in world frame. Pose3 wTc = world_P_body * body_P_sensor; cameras.push_back(PinholeCamera(wTc)); - + // Simple point to project slightly off image center Point3 p(0, 0, 10); Point2 measurement = cameras[0].project(p); @@ -81,9 +63,10 @@ TEST(SmartFactorBase, PinholeWithSensor) { Matrix E; Vector error = f.unwhitenedError(cameras, p, Fs, E); - Vector expectedError = Vector::Zero(2); + Vector expectedError = Vector::Zero(2); Matrix29 expectedFs; - expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0; + expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, + 0, 0, 0, 0; Matrix23 expectedE; expectedE << 0.1, 0, 0.01, 0, 0.1, 0; @@ -94,20 +77,13 @@ TEST(SmartFactorBase, PinholeWithSensor) { EXPECT(assert_equal(expectedE, E)); } -/* ************************************************************************* */ -#include - -namespace gtsam { - -class StereoFactor: public SmartFactorBase { -public: +class StereoFactor : public SmartFactorBase { + public: typedef SmartFactorBase Base; StereoFactor() {} - StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { - } - double error(const Values& values) const override { - return 0.0; - } + StereoFactor(const SharedNoiseModel& sharedNoiseModel) + : Base(sharedNoiseModel) {} + double error(const Values& values) const override { return 0.0; } boost::shared_ptr linearize( const Values& values) const override { return boost::shared_ptr(new JacobianFactor()); @@ -115,9 +91,8 @@ public: }; /// traits -template<> +template <> struct traits : public Testable {}; -} TEST(SmartFactorBase, Stereo) { StereoFactor f(unit3); @@ -125,24 +100,7 @@ TEST(SmartFactorBase, Stereo) { f.add(StereoPoint2(), 2); EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } - -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") - -TEST(SmartFactorBase, serialize) { - using namespace gtsam::serializationTestHelpers; - PinholeFactor factor(unit2); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} +} // namespace gtsam /* ************************************************************************* */ int main() { @@ -150,4 +108,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 133f81511..ecdb5287f 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -22,18 +22,19 @@ #include "smartFactorScenarios.h" #include #include -#include #include #include #include using namespace boost::assign; +namespace { static const bool isDebugTest = false; static const Symbol l1('l', 1), l2('l', 2), l3('l', 3); static const Key c1 = 1, c2 = 2, c3 = 3; static const Point2 measurement1(323.0, 240.0); static const double rankTol = 1.0; +} template PinholeCamera perturbCameraPoseAndCalibration( @@ -70,8 +71,9 @@ TEST(SmartProjectionFactor, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionFactor, Constructor2) { using namespace vanilla; - params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, params); + auto myParams = params; + myParams.setRankTolerance(rankTol); + SmartFactor factor1(unit2, myParams); } /* ************************************************************************* */ @@ -84,8 +86,9 @@ TEST(SmartProjectionFactor, Constructor3) { /* ************************************************************************* */ TEST(SmartProjectionFactor, Constructor4) { using namespace vanilla; - params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, params); + auto myParams = params; + myParams.setRankTolerance(rankTol); + SmartFactor factor1(unit2, myParams); factor1.add(measurement1, c1); } @@ -810,25 +813,6 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { EXPECT(assert_equal(yActual, yExpected, 1e-7)); } -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") - -TEST(SmartProjectionFactor, serialize) { - using namespace vanilla; - using namespace gtsam::serializationTestHelpers; - SmartFactor factor(unit2); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f3706fa02..5c38233c1 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include @@ -32,6 +31,7 @@ using namespace boost::assign; using namespace std::placeholders; +namespace { static const double rankTol = 1.0; // Create a noise model for the pixel error static const double sigma = 0.1; @@ -51,6 +51,7 @@ static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): // lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY; +} /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { @@ -1332,47 +1333,6 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.at(x3))); } -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") - -TEST(SmartProjectionPoseFactor, serialize) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactor factor(model, sharedK, params); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - -TEST(SmartProjectionPoseFactor, serialize2) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - Pose3 bts; - SmartFactor factor(model, sharedK, bts, params); - - // insert some measurments - KeyVector key_view; - Point2Vector meas_view; - key_view.push_back(Symbol('x', 1)); - meas_view.push_back(Point2(10, 10)); - factor.add(meas_view, key_view); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/symbolic/SymbolicJunctionTree.h b/gtsam/symbolic/SymbolicJunctionTree.h index 7a152e532..0dcfae541 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.h +++ b/gtsam/symbolic/SymbolicJunctionTree.h @@ -16,6 +16,8 @@ * @author Richard Roberts */ +#pragma once + #include #include #include diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 33fc3243b..ee9b41a5a 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -731,10 +731,12 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { { Ordering ordering = Ordering::Create(Ordering::METIS, sfg); // Linux and Mac split differently when using mettis -#if !defined(__APPLE__) - EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering)); -#else +#if defined(__APPLE__) EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering)); +#elif defined(_WIN32) + EXPECT(assert_equal(Ordering(list_of(4)(3)(1)(0)(5)(2)), ordering)); +#else + EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering)); #endif // - P( 1 0 3) @@ -742,20 +744,27 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | | - P( 5 | 0 4) // | - P( 2 | 1 3) SymbolicBayesTree expected; -#if !defined(__APPLE__) - expected.insertRoot( - MakeClique(list_of(2)(4)(1), 3, - list_of( - MakeClique(list_of(0)(1)(4), 1, - list_of(MakeClique(list_of(5)(0)(4), 1))))( - MakeClique(list_of(3)(2)(4), 1)))); -#else +#if defined(__APPLE__) expected.insertRoot( MakeClique(list_of(1)(0)(3), 3, list_of( MakeClique(list_of(4)(0)(3), 1, list_of(MakeClique(list_of(5)(0)(4), 1))))( MakeClique(list_of(2)(1)(3), 1)))); +#elif defined(_WIN32) + expected.insertRoot( + MakeClique(list_of(3)(5)(2), 3, + list_of( + MakeClique(list_of(4)(3)(5), 1, + list_of(MakeClique(list_of(0)(2)(5), 1))))( + MakeClique(list_of(1)(0)(2), 1)))); +#else + expected.insertRoot( + MakeClique(list_of(2)(4)(1), 3, + list_of( + MakeClique(list_of(0)(1)(4), 1, + list_of(MakeClique(list_of(5)(0)(4), 1))))( + MakeClique(list_of(3)(2)(4), 1)))); #endif SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index 9d854a169..94e27d6c4 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -17,6 +17,8 @@ * @date Feb 3, 2010 */ +#pragma once + #include #include #include diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h index a2f544de5..548bce344 100644 --- a/gtsam_unstable/base/Dummy.h +++ b/gtsam_unstable/base/Dummy.h @@ -17,6 +17,8 @@ * @date June 14, 2012 */ +#pragma once + #include #include #include diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 4ee7b85eb..168891e6f 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -34,7 +34,7 @@ using Domains = std::map; * Base class for constraint factors * Derived classes include SingleValue, BinaryAllDiff, and AllDiff. */ -class GTSAM_EXPORT Constraint : public DiscreteFactor { +class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { public: typedef boost::shared_ptr shared_ptr; diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h index 12374ac76..350985cf4 100644 --- a/gtsam_unstable/linear/ActiveSetSolver-inl.h +++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h @@ -17,6 +17,8 @@ * @date 2/11/16 */ +#pragma once + #include /******************************************************************************/ @@ -283,4 +285,4 @@ Template std::pair This::optimize() const { } #undef Template -#undef This \ No newline at end of file +#undef This diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 460b4b7ee..f36462bda 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -17,6 +17,8 @@ * @date 6/16/16 */ +#pragma once + #include #include #include diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 3854d2a15..ae87b3ab7 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -17,6 +17,8 @@ * @date 6/16/16 */ +#pragma once + #include #include #include @@ -45,4 +47,4 @@ struct QPPolicy { using QPSolver = ActiveSetSolver; -} \ No newline at end of file +} diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 1a86adbfa..0c821b872 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -16,6 +16,7 @@ */ #include +#include #include namespace gtsam { diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 2e48b0d45..0e4950b79 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -20,11 +20,10 @@ #include "FindSeparator.h" -#ifndef GTSAM_USE_SYSTEM_METIS +#include extern "C" { -#include -#include "metislib.h" +#include } @@ -566,5 +565,3 @@ namespace gtsam { namespace partition { } }} //namespace - -#endif diff --git a/gtsam_unstable/partition/FindSeparator.h b/gtsam_unstable/partition/FindSeparator.h index 42d971a82..f4342695b 100644 --- a/gtsam_unstable/partition/FindSeparator.h +++ b/gtsam_unstable/partition/FindSeparator.h @@ -6,6 +6,8 @@ * Description: find the separator of bisectioning for a given graph */ +#pragma once + #include #include #include diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index 63acc8f18..fe49de928 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -20,8 +20,6 @@ using namespace std; using namespace gtsam; using namespace gtsam::partition; -#ifndef GTSAM_USE_SYSTEM_METIS - /* ************************************************************************* */ // x0 - x1 - x2 // l3 l4 @@ -229,8 +227,6 @@ TEST ( Partition, findSeparator3_with_reduced_camera ) LONGS_EQUAL(2, partitionTable[28]); } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 35b4677d5..714e62288 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -5,8 +5,7 @@ * Author: cbeall3 */ -#ifndef AHRS_H_ -#define AHRS_H_ +#pragma once #include "Mechanization_bRn2.h" #include @@ -82,4 +81,3 @@ public: }; } /* namespace gtsam */ -#endif /* AHRS_H_ */ diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 5cdfb2ab7..61f110d3a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -49,7 +49,7 @@ typedef SmartProjectionParams SmartStereoProjectionParams; * If you'd like to store poses in values instead of cameras, use * SmartStereoProjectionPoseFactor instead */ -class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactor +class SmartStereoProjectionFactor : public SmartFactorBase { private: diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index e9157317e..362cf3778 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -7,9 +7,6 @@ * @author Alex Cunningham */ -#include -#include - #include #include @@ -18,12 +15,16 @@ #include #include -#include -#include -#include +#include + #include #include +#include +#include +#include +#include + using namespace std; using namespace gtsam; using namespace boost::assign; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp index 61836d098..c71c19038 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp @@ -31,12 +31,13 @@ using namespace std; using namespace boost::assign; using namespace gtsam; +namespace { // make a realistic calibration matrix static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); -static Cal3_S2Stereo::shared_ptr K2( - new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); +static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, + b)); static SmartStereoProjectionParams params; @@ -45,8 +46,8 @@ static SmartStereoProjectionParams params; static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -59,16 +60,19 @@ static Symbol body_P_cam3_key('P', 3); static Key poseKey1(x1); static Key poseExtrinsicKey1(body_P_cam1_key); static Key poseExtrinsicKey2(body_P_cam2_key); -static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? -static StereoPoint2 measurement2(350.0, 200.0, 240.0); //potentially use more reasonable measurement value? +static StereoPoint2 measurement1( + 323.0, 300.0, 240.0); // potentially use more reasonable measurement value? +static StereoPoint2 measurement2( + 350.0, 200.0, 240.0); // potentially use more reasonable measurement value? static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); + Point3(0.25, -0.10, 1.0)); static double missing_uR = std::numeric_limits::quiet_NaN(); vector stereo_projectToMultipleCameras(const StereoCamera& cam1, - const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { - + const StereoCamera& cam2, + const StereoCamera& cam3, + Point3 landmark) { vector measurements_cam; StereoPoint2 cam1_uv1 = cam1.project(landmark); @@ -82,6 +86,7 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, } LevenbergMarquardtParams lm_params; +} // namespace /* ************************************************************************* */ TEST( SmartStereoProjectionFactorPP, params) { diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index a0bfc3649..fc56b1a9f 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -32,13 +32,13 @@ using namespace std; using namespace boost::assign; using namespace gtsam; +namespace { // make a realistic calibration matrix static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); -static Cal3_S2Stereo::shared_ptr K2( - new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); - +static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, + b)); static SmartStereoProjectionParams params; @@ -47,8 +47,8 @@ static SmartStereoProjectionParams params; static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -56,15 +56,17 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Key poseKey1(x1); -static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? +static StereoPoint2 measurement1( + 323.0, 300.0, 240.0); // potentially use more reasonable measurement value? static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); + Point3(0.25, -0.10, 1.0)); static double missing_uR = std::numeric_limits::quiet_NaN(); vector stereo_projectToMultipleCameras(const StereoCamera& cam1, - const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { - + const StereoCamera& cam2, + const StereoCamera& cam3, + Point3 landmark) { vector measurements_cam; StereoPoint2 cam1_uv1 = cam1.project(landmark); @@ -78,6 +80,7 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, } LevenbergMarquardtParams lm_params; +} // namespace /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, params) { diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 1755e2075..a657c6be7 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -92,8 +92,8 @@ if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) endif() # Wrap - matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam" "" - "${mexFlags}" "${ignore}") + matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam_unstable" + "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}" "${ignore}") endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) # Record the root dir for gtsam - needed during external builds, e.g., ROS diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 85ddc7b6d..f5869b145 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -172,7 +172,7 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install + COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) diff --git a/python/gtsam/examples/RangeISAMExample_plaza2.ipynb b/python/gtsam/examples/RangeISAMExample_plaza2.ipynb new file mode 100644 index 000000000..f11636606 --- /dev/null +++ b/python/gtsam/examples/RangeISAMExample_plaza2.ipynb @@ -0,0 +1,9453 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information\n", + "\n", + "A 2D Range SLAM example, with iSAM and smart range factors\n", + "\n", + "Author: Frank Dellaert" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Data is second UWB ranging dataset, B2 or \"plaza 2\", from\n", + "\n", + "> \"Navigating with Ranging Radios: Five Data Sets with Ground Truth\", by Joseph Djugash, Bradley Hamner, and Stephan Roth, available at https://www.ri.cmu.edu/pub_files/2009/9/Final_5datasetsRangingRadios.pdf\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": 49, + "metadata": {}, + "outputs": [], + "source": [ + "# pylint: disable=invalid-name, E1101\n", + "\n", + "from gtsam import Point2, Pose2\n", + "import plotly.express as px\n", + "import numpy as np\n", + "import gtsam\n", + "import math\n", + "\n", + "import matplotlib.pyplot as plt\n", + "from gtsam.utils import plot\n", + "from numpy.random import default_rng\n", + "\n", + "rng = default_rng()\n", + "\n", + "NM = gtsam.noiseModel" + ] + }, + { + "cell_type": "code", + "execution_count": 50, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Read 4090 odometry entries.\n" + ] + } + ], + "source": [ + "# load the odometry\n", + "# DR: Odometry Input (delta distance traveled and delta heading change)\n", + "# Time (sec) Delta Distance Traveled (m) Delta Heading (rad)\n", + "odometry = {}\n", + "data_file = gtsam.findExampleDataFile(\"Plaza2_DR.txt\")\n", + "for row in np.loadtxt(data_file):\n", + " t, distance_traveled, delta_heading = row\n", + " odometry[t] = Pose2(distance_traveled, 0, delta_heading)\n", + "M = len(odometry)\n", + "print(f\"Read {M} odometry entries.\")" + ] + }, + { + "cell_type": "code", + "execution_count": 51, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Read 1816 range triples.\n" + ] + } + ], + "source": [ + "# load the ranges from TD\n", + "# Time (sec) Sender / Antenna ID Receiver Node ID Range (m)\n", + "triples = []\n", + "data_file = gtsam.findExampleDataFile(\"Plaza2_TD.txt\")\n", + "for row in np.loadtxt(data_file):\n", + " t, sender, receiver, _range = row\n", + " triples.append((t, int(receiver), _range))\n", + "K = len(triples)\n", + "print(f\"Read {K} range triples.\")" + ] + }, + { + "cell_type": "code", + "execution_count": 52, + "metadata": {}, + "outputs": [], + "source": [ + "# parameters\n", + "minK = 150 # minimum number of range measurements to process initially\n", + "incK = 25 # minimum number of range measurements to process after\n", + "robust = True" + ] + }, + { + "cell_type": "code", + "execution_count": 53, + "metadata": {}, + "outputs": [], + "source": [ + "# Set Noise parameters\n", + "priorSigmas = gtsam.Point3(1, 1, math.pi)\n", + "odoSigmas = gtsam.Point3(0.05, 0.01, 0.1)\n", + "sigmaR = 100 # range standard deviation\n", + "\n", + "priorNoise = NM.Diagonal.Sigmas(priorSigmas) # prior\n", + "looseNoise = NM.Isotropic.Sigma(2, 1000) # loose LM prior\n", + "odoNoise = NM.Diagonal.Sigmas(odoSigmas) # odometry\n", + "gaussian = NM.Isotropic.Sigma(1, sigmaR) # non-robust\n", + "tukey = NM.Robust.Create(NM.mEstimator.Tukey.Create(15), gaussian) # robust\n", + "rangeNoise = tukey if robust else gaussian" + ] + }, + { + "cell_type": "code", + "execution_count": 54, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + ": cliques: 0, variables: 0\n", + "\n" + ] + } + ], + "source": [ + "# Initialize iSAM\n", + "isam = gtsam.ISAM2()\n", + "print(isam)" + ] + }, + { + "cell_type": "code", + "execution_count": 55, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "NonlinearFactorGraph: size: 1\n", + "\n", + "Factor 0: PriorFactor on 0\n", + " prior mean: (-34.208649, 45.300764, 1.12050365)\n", + " noise model: diagonal sigmas[1; 1; 3.14159265];\n", + "\n", + " Values with 1 values:\n", + "Value 0: (gtsam::Pose2)\n", + "(-34.208649, 45.300764, 1.12050365)\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# Add prior on first pose\n", + "pose0 = Pose2(-34.2086489999201, 45.3007639991120, math.pi - 2.021089)\n", + "newFactors = gtsam.NonlinearFactorGraph()\n", + "newFactors.addPriorPose2(0, pose0, priorNoise)\n", + "initial = gtsam.Values()\n", + "initial.insert(0, pose0)\n", + "print(newFactors, initial)" + ] + }, + { + "cell_type": "code", + "execution_count": 56, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Adding landmark L1\n", + "Adding landmark L6\n", + "Adding landmark L0\n", + "Adding landmark L5\n", + "Initializing at time 151\n" + ] + } + ], + "source": [ + "# set some loop variables\n", + "i = 1 # step counter\n", + "k = 0 # range measurement counter\n", + "initialized = False\n", + "lastPose = pose0\n", + "countK = 0\n", + "\n", + "initializedLandmarks = set()\n", + "\n", + "# Loop over odometry\n", + "for t, relative_pose in odometry.items():\n", + " # add odometry factor\n", + " newFactors.add(gtsam.BetweenFactorPose2(i - 1, i, relative_pose,\n", + " odoNoise))\n", + "\n", + " # predict pose and add as initial estimate\n", + " predictedPose = lastPose.compose(relative_pose)\n", + " lastPose = predictedPose\n", + " initial.insert(i, predictedPose)\n", + "\n", + " # Check if there are range factors to be added\n", + " while (k < K) and (triples[k][0] <= t):\n", + " j = triples[k][1]\n", + " landmark_key = gtsam.symbol('L', j)\n", + " _range = triples[k][2]\n", + " newFactors.add(gtsam.RangeFactor2D(\n", + " i, landmark_key, _range, rangeNoise))\n", + " if landmark_key not in initializedLandmarks:\n", + " p = rng.normal(loc=0, scale=100, size=(2,))\n", + " initial.insert(landmark_key, p)\n", + " print(f\"Adding landmark L{j}\")\n", + " initializedLandmarks.add(landmark_key)\n", + " # We also add a very loose prior on the landmark in case there is only\n", + " # one sighting, which cannot fully determine the landmark.\n", + " newFactors.add(gtsam.PriorFactorPoint2(\n", + " landmark_key, Point2(0, 0), looseNoise))\n", + " k = k + 1\n", + " countK = countK + 1\n", + "\n", + " # Check whether to update iSAM 2\n", + " if (k > minK) and (countK > incK):\n", + " if not initialized: # Do a full optimize for first minK ranges\n", + " print(f\"Initializing at time {k}\")\n", + " batchOptimizer = gtsam.LevenbergMarquardtOptimizer(\n", + " newFactors, initial)\n", + " initial = batchOptimizer.optimize()\n", + " initialized = True\n", + "\n", + " isam.update(newFactors, initial)\n", + " result = isam.calculateEstimate()\n", + " lastPose = result.atPose2(i)\n", + " newFactors = gtsam.NonlinearFactorGraph()\n", + " initial = gtsam.Values()\n", + " countK = 0\n", + "\n", + " i += 1\n", + "\n", + "finalResult = isam.calculateEstimate()" + ] + }, + { + "cell_type": "code", + "execution_count": 57, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "5476377146882523136: [-35.97329685 26.31658086]\n", + "5476377146882523137: [-75.1003452 21.01144091]\n", + "5476377146882523141: [ -1.03876425 -12.13811931]\n", + "5476377146882523142: [-36.08926944 72.3500464 ]\n" + ] + } + ], + "source": [ + "# Print optimized landmarks:\n", + "for j in [0,1,5,6]:\n", + " landmark_key = gtsam.symbol('L', j)\n", + " p = finalResult.atPoint2(landmark_key)\n", + " print(f\"{landmark_key}: {p}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 58, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(4090, 2)\n" + ] + } + ], + "source": [ + "# plot positions\n", + "poses = gtsam.utilities.allPose2s(finalResult)\n", + "landmarks = gtsam.utilities.extractPoint2(finalResult)\n", + "positions = np.array([poses.atPose2(key).translation()\n", + " for key in poses.keys()])\n", + "print(positions.shape)" + ] + }, + { + "cell_type": "code", + "execution_count": 59, + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "hovertemplate": "x=%{x}
y=%{y}", + "legendgroup": "", + "marker": { + "color": "#636efa", + "symbol": "circle" + }, + "mode": "markers", + "name": "", + "showlegend": false, + "type": "scattergl", + "x": [ + -34.20850096446654, + -34.208238954743685, + -34.20792902100424, + -34.20760757184568, + -34.20738361846996, + -34.20720176000561, + -34.20683416597183, + -34.20650648822083, + -34.20608935588558, + -34.20572966544402, + -34.20539047307245, + -34.204978189129015, + -34.204550257854706, + -34.20419937583317, + -34.203851648593165, + -34.20343169052313, + -34.203042274235045, + -34.20264363032312, + -34.2023726490794, + -34.202098140666784, + -34.20181134675162, + -34.20148933862536, + -34.20121738216338, + -34.2009080994932, + -34.20063643983228, + -34.20034481742374, + -34.2000696237222, + -34.19983311428898, + -34.19961392234484, + -34.19937365514169, + -34.199189402854074, + -34.19900317603461, + -34.19880114105563, + -34.19861066740314, + -34.19837645150039, + -34.19816029290118, + -34.19797297627397, + -34.19781332693034, + -34.197621612689275, + -34.19741383124952, + -34.19727147453903, + -34.19707929682523, + -34.196825570993276, + -34.19668981102975, + -34.1965642221961, + -34.196420248329694, + -34.196235418026966, + -34.196100527762006, + -34.19595528348216, + -34.19577717294135, + -34.19561281660328, + -34.19541564722681, + -34.19522091075678, + -34.19506867319604, + -34.19491322906737, + -34.19475617348167, + -34.19456738946074, + -34.19441257759611, + -34.194305608523415, + -34.19413890108838, + -34.19402563257533, + -34.193871851019416, + -34.19370503405217, + -34.19358027267171, + -34.19344531751981, + -34.19329006230464, + -34.19314564970173, + -34.19281770202517, + -34.19237269478954, + -34.19196721038553, + -34.191578273729284, + -34.19110684869285, + -34.19059806524432, + -34.19014443601172, + -34.18971520292186, + -34.18923602362348, + -34.18869913427268, + -34.18833003245038, + -34.18794166622675, + -34.18751483624891, + -34.187190230565314, + -34.18680050834423, + -34.18632317786081, + -34.185942639398604, + -34.18557941319778, + -34.185093513453985, + -34.18464292833133, + -34.184145833297634, + -34.1835690413308, + -34.183009218187, + -34.18245641784577, + -34.18194165756511, + -34.181411245583405, + -34.180842191387, + -34.18031070625432, + -34.17977878608106, + -34.179274141905964, + -34.178791235572014, + -34.17844353743938, + -34.178133542359454, + -34.17772043770016, + -34.17732671873731, + -34.17705071666225, + -34.17674574279609, + -34.17648907354066, + -34.17609695070887, + -34.175650899786554, + -34.17532133596385, + -34.1749278740639, + -34.1745363562516, + -34.17415109707245, + -34.17368635309034, + -34.173255122177345, + -34.172899756560206, + -34.17251040591449, + -34.172086874520765, + -34.171630360950736, + -34.17117119532719, + -34.17084558026226, + -34.170524084280146, + -34.170110063374906, + -34.16967258385827, + -34.16930042973883, + -34.168970613859635, + -34.16856304460919, + -34.16811576085387, + -34.16768676123481, + -34.16736099295385, + -34.167124898097356, + -34.16672691923807, + -34.16629949645637, + -34.166020544017336, + -34.16567348824885, + -34.165206629486555, + -34.1649102522274, + -34.16442554900806, + -34.16411656055334, + -34.163835743037595, + -34.163667960830786, + -34.163287829216934, + -34.16288151302674, + -34.16269395430048, + -34.16238991455418, + -34.162026429660955, + -34.161650280510415, + -34.16120406524903, + -34.16081468768969, + -34.16051969428689, + -34.16009094929095, + -34.15946914274596, + -34.159176862117704, + -34.15888727213249, + -34.15848397635331, + -34.15805064953907, + -34.15768222447644, + -34.15741075690409, + -34.157199236984766, + -34.15702885116569, + -34.156646530479655, + -34.15631433417576, + -34.15610539694631, + -34.155873450868484, + -34.155492955086224, + -34.15500939848627, + -34.15434250697164, + -34.15403249529436, + -34.15373559707832, + -34.153195077612814, + -34.15237223406272, + -34.151197823002306, + -34.150389061340434, + -34.14972355532398, + -34.14876182425223, + -34.147753205875084, + -34.1468315985221, + -34.14592332052173, + -34.14499865748368, + -34.14413081753902, + -34.14326697595306, + -34.14235363131009, + -34.141504566972635, + -34.140670341648466, + -34.13977560684852, + -34.138843663846956, + -34.13797352118324, + -34.13704209938618, + -34.136104337300594, + -34.13535680475344, + -34.13475717277114, + -34.13405664376714, + -34.13324167350189, + -34.13239793097516, + -34.13162108183035, + -34.13104483686513, + -34.130370325655754, + -34.129575170192076, + -34.12883978107497, + -34.12818004157837, + -34.12769342542458, + -34.12707652242479, + -34.12625640229815, + -34.12573308632649, + -34.1251498038307, + -34.124445847032156, + -34.12353836877944, + -34.122053316887815, + -34.1211257367895, + -34.119724360218534, + -34.11843319269593, + -34.11718493708246, + -34.11338507210205, + -34.10590214862871, + -34.09282073690137, + -34.07236785491298, + -34.04366888084793, + -34.007605138373535, + -33.96775324384806, + -33.92893317560639, + -33.89642176487315, + -33.87043613439064, + -33.85153191238244, + -33.83740516873628, + -33.82726615508978, + -33.820250147386204, + -33.81406643483115, + -33.806553260040644, + -33.79553734291343, + -33.77741136696219, + -33.751353316306194, + -33.71687278795068, + -33.673404994787944, + -33.62118686393082, + -33.56068922257415, + -33.49143699335243, + -33.412834311016205, + -33.3237099791763, + -33.22310971008737, + -33.11154866410422, + -32.990842000614734, + -32.86212511539496, + -32.72309606849177, + -32.57062017013406, + -32.40851697353658, + -32.23720250997644, + -32.05784746076087, + -31.866728622723116, + -31.666281580501536, + -31.45888722393481, + -31.236188791114166, + -31.006924763292396, + -30.77239936272471, + -30.52223313580673, + -30.2651984576044, + -30.000474598875655, + -29.731386707719196, + -29.452770053026384, + -29.16946897839721, + -28.89232263981253, + -28.60861583098035, + -28.320690784708873, + -28.03548898720037, + -27.750887893729637, + -27.462116131387482, + -27.179759975432518, + -26.903824475123297, + -26.631328055718043, + -26.36043103504407, + -26.096833531141332, + -25.850422454635652, + -25.602392293361373, + -25.35759661816489, + -25.14796974324249, + -24.936472171543887, + -24.729593124054873, + -24.537083022661065, + -24.353155730789634, + -24.178930832936064, + -24.00913393888203, + -23.839729121837294, + -23.687506229039517, + -23.537568073364582, + -23.385293532067568, + -23.247396754879265, + -23.113088745898345, + -22.97788261894402, + -22.85033600215608, + -22.7302817180564, + -22.614875683411867, + -22.501606043047857, + -22.39296302095069, + -22.291124620939094, + -22.1931172182229, + -22.10259642363014, + -22.01442660431957, + -21.930835891539587, + -21.854838952711773, + -21.785225767526434, + -21.71575024868301, + -21.648654064018306, + -21.584573001491112, + -21.520141220887233, + -21.45177011272198, + -21.384492813439486, + -21.318955993291997, + -21.25214802678836, + -21.183657070739866, + -21.119719849680337, + -21.054127260365053, + -20.983247893375697, + -20.91436578568897, + -20.84846061043535, + -20.77885808389792, + -20.70787520393427, + -20.64043121965617, + -20.567976117179327, + -20.49802638852945, + -20.425461212722524, + -20.35479298424054, + -20.282674033670123, + -20.20903252554625, + -20.137397231124215, + -20.064022256051153, + -19.989043931523284, + -19.90910114815038, + -19.83232039990529, + -19.753511678281324, + -19.669105733913366, + -19.583314519263745, + -19.494616842451236, + -19.40443270134451, + -19.310538831842955, + -19.213843720805905, + -19.115886189028437, + -19.01472079114958, + -18.911519476095137, + -18.806724464509383, + -18.699383897195215, + -18.587828190276884, + -18.475139327437294, + -18.358508133023715, + -18.23250049997659, + -18.107209041615686, + -17.97898633278654, + -17.839883042643052, + -17.69648363507199, + -17.551859605023964, + -17.39543380713658, + -17.234970818650286, + -17.070046627768086, + -16.896655854204827, + -16.72189078956723, + -16.53903862823957, + -16.345578366064313, + -16.15040970857468, + -15.949004036301039, + -15.73409882318099, + -15.520776286461096, + -15.301925922368234, + -15.070678004620648, + -14.843307150651425, + -14.613679045242476, + -14.375157473101241, + -14.139389072791404, + -13.904988476433443, + -13.661150612368795, + -13.41707736541694, + -13.180820011095541, + -12.940189575595827, + -12.700155724224805, + -12.46287048059889, + -12.225978074095101, + -11.983676868668432, + -11.741119724603255, + -11.498800839246972, + -11.257118257374717, + -11.01223198786857, + -10.771316621036691, + -10.531816609803048, + -10.289242645492118, + -10.051094210070056, + -9.813321328273139, + -9.572194435045677, + -9.335290205071642, + -9.103435364388192, + -8.870725952526021, + -8.643668752575262, + -8.425262228892867, + -8.180860199005918, + -7.957839554013466, + -7.742057345061757, + -7.528013744629356, + -7.314109479110836, + -7.103808525252691, + -6.898801427447551, + -6.693570821226059, + -6.493186831966468, + -6.296214106306464, + -6.099657869405394, + -5.908827113614065, + -5.715615709860129, + -5.521984170694302, + -5.332461478571686, + -5.143684708007091, + -4.953569959626599, + -4.772259464875152, + -4.599443535946358, + -4.422658612841595, + -4.25775388602834, + -4.1065418176812125, + -3.954217993435107, + -3.8120387751928058, + -3.684418011623918, + -3.560233442270346, + -3.4467609954925646, + -3.347455125665888, + -3.256983334164835, + -3.1818452917066, + -3.119373397337299, + -3.0677831078079105, + -3.031257649766328, + -3.0084601096112937, + -2.999507935999349, + -3.0054698298286207, + -3.0281236342209943, + -3.0700352925929155, + -3.128581122683889, + -3.204051529859299, + -3.29715812617134, + -3.410492761151179, + -3.548730766585928, + -3.7148238763535497, + -3.892215883500358, + -4.08410985314791, + -4.303713350962656, + -4.53199807872445, + -4.769603912150116, + -5.039499581466397, + -5.317875891463045, + -5.594221969919161, + -5.890632592115414, + -6.198945169797842, + -6.511674328245961, + -6.833766739908534, + -7.159226307604196, + -7.485880834688631, + -7.8213576057842005, + -8.155947721189817, + -8.490831353136372, + -8.82915319130515, + -9.167136649612592, + -9.50351549191835, + -9.844389215804224, + -10.179525657340298, + -10.515272701348366, + -10.853513119276752, + -11.186384006918765, + -11.51885458278919, + -11.853887739779578, + -12.186218771001966, + -12.518838054018211, + -12.85379646413921, + -13.192524168500752, + -13.531297475935212, + -13.87278381731817, + -14.216843064780582, + -14.565137202540603, + -14.916964860695987, + -15.276638133253954, + -15.649521404743567, + -16.023504645043484, + -16.399218438899965, + -16.787539834221676, + -17.17487956619213, + -17.55780193233755, + -17.953415497509326, + -18.36193162941352, + -18.753242349393854, + -19.1547288146283, + -19.57488363759125, + -19.97196405956268, + -20.382959986893194, + -20.808590477926927, + -21.214446333859488, + -21.63333896306491, + -22.063312427305817, + -22.469330277045458, + -22.883562574578935, + -23.30562305116361, + -23.69693990260586, + -24.089484082222096, + -24.508476133226807, + -24.91173666527541, + -25.300314654764954, + -25.714811859707304, + -26.12548731270093, + -26.513524431697025, + -26.921808472447154, + -27.335390292031736, + -27.730896380438267, + -28.135096223390125, + -28.54305955816263, + -28.936817600015, + -29.33587430829889, + -29.735739337632634, + -30.131784852729698, + -30.531510034799926, + -30.931020949816958, + -31.328154826694753, + -31.73203776645058, + -32.134486553878176, + -32.54050706752538, + -32.949192934150325, + -33.36421956031278, + -33.7768751678339, + -34.18940158319885, + -34.6021492023042, + -35.01047415651771, + -35.41171781082104, + -35.813119692813984, + -36.215050467039944, + -36.621893345222226, + -37.03717132871312, + -37.46002493725777, + -37.883741357496945, + -38.30719995932858, + -38.72938025987588, + -39.14924357296815, + -39.57802315670335, + -40.01043348825386, + -40.439590791865776, + -40.85750222093139, + -41.27182723776138, + -41.68831190734649, + -42.104015239877675, + -42.52170843545669, + -42.93599499431075, + -43.354357210777145, + -43.7750624616118, + -44.180848445444646, + -44.58812097321755, + -45.007713562651716, + -45.417312323038956, + -45.82621709698976, + -46.24179036897837, + -46.659494896046056, + -47.0695288423755, + -47.476973756636134, + -47.88445074356961, + -48.28357380372633, + -48.67844143188854, + -49.08561638451694, + -49.48706722067271, + -49.877482019089115, + -50.279156256808044, + -50.676273600636236, + -51.053999874666324, + -51.447405905939775, + -51.835994120984935, + -52.21242997655724, + -52.60506604706158, + -52.998686597981106, + -53.37501814955399, + -53.768167517412685, + -54.159168318936786, + -54.53551172277161, + -54.92985669052983, + -55.31894439666703, + -55.69133528047126, + -56.07482392120389, + -56.444863159140716, + -56.81320710027343, + -57.19053508956895, + -57.56215251701041, + -57.93099347502464, + -58.30172428484685, + -58.664305554440965, + -59.025359706567336, + -59.39829137353854, + -59.7582141542276, + -60.124530748974514, + -60.4908578766057, + -60.83505696904816, + -61.18130885226007, + -61.52499966132512, + -61.83836490363596, + -62.158196325612444, + -62.47435398172692, + -62.76534189619135, + -63.05109916028611, + -63.32856523564792, + -63.59049106119492, + -63.83443997198932, + -64.07088373841837, + -64.29533435273933, + -64.50526421490805, + -64.7052822956055, + -64.88974984865094, + -65.06757453538047, + -65.23988864735162, + -65.39869442920023, + -65.54776860045122, + -65.68685907210161, + -65.81838416625914, + -65.9438257267132, + -66.0586877840251, + -66.16983067614979, + -66.2802816919717, + -66.3794524945481, + -66.47116560935949, + -66.5580783084223, + -66.63414777564289, + -66.7028493866808, + -66.76637121729858, + -66.81648907329419, + -66.85396394868584, + -66.88622644304414, + -66.9069131925393, + -66.91060786059475, + -66.89911095628779, + -66.87390242235816, + -66.83644811340965, + -66.7854338571656, + -66.71980373895558, + -66.6393075249817, + -66.54347632615395, + -66.43556800084674, + -66.31751313422161, + -66.1879628899378, + -66.0492213591998, + -65.90249473180852, + -65.74234707643778, + -65.56934163451142, + -65.38876145051714, + -65.20278639422463, + -65.00492563650491, + -64.80032711231964, + -64.60508760719165, + -64.39640405531888, + -64.17347324859307, + -63.96347278695096, + -63.74783369003469, + -63.50827319197837, + -63.27817201058187, + -63.049125731267196, + -62.79906825605081, + -62.55571846530885, + -62.314009308642575, + -62.06057816611769, + -61.812002866040984, + -61.568008672865844, + -61.31977697955609, + -61.07544160626518, + -60.83338909845368, + -60.58843697818208, + -60.342020309956226, + -60.0982834257727, + -59.854435946413936, + -59.614697844411104, + -59.37731347930396, + -59.1442368134974, + -58.907917683733785, + -58.67663687502402, + -58.44415651110442, + -58.2105060936788, + -57.98171333830625, + -57.75986034582937, + -57.53099620873651, + -57.31742354180103, + -57.11094708789625, + -56.89404473721648, + -56.686883256420124, + -56.47789349938912, + -56.25906193982285, + -56.04874031747729, + -55.838189439977356, + -55.60030507722978, + -55.389217653260076, + -55.18088735036473, + -54.964419297405115, + -54.74251225436768, + -54.526641540244455, + -54.309039392756404, + -54.08466206621436, + -53.864323502833976, + -53.64917961946586, + -53.42760336382845, + -53.20497730544819, + -52.98581775877011, + -52.76052182351032, + -52.53402751945837, + -52.3150187111637, + -52.09446139112003, + -51.875301855406015, + -51.66458491630855, + -51.44880184949696, + -51.23583887481144, + -51.024251639441495, + -50.8071041651678, + -50.584758876264836, + -50.35673439549038, + -50.1297573599387, + -49.89796426630089, + -49.6561733825751, + -49.408897775010146, + -49.158956206097, + -48.907045966962926, + -48.65259274481218, + -48.39660932186194, + -48.14165349692995, + -47.89462459547674, + -47.649591173534326, + -47.40018989835032, + -47.153616860328476, + -46.910162188884115, + -46.66125059392396, + -46.41004047357298, + -46.160765219588185, + -45.91311081595627, + -45.66671801274094, + -45.40747481662267, + -45.15426593717196, + -44.909388930303805, + -44.647962917352636, + -44.38667975642351, + -44.146999413931816, + -43.889502030617585, + -43.61894147724354, + -43.365040674727965, + -43.10397341963339, + -42.82173856681402, + -42.54942670163961, + -42.29427549203657, + -42.01245170382104, + -41.72086995614185, + -41.44495855163145, + -41.14649300925449, + -40.83513244019241, + -40.53309408854726, + -40.217082860406435, + -39.899885571661386, + -39.58322762333115, + -39.25927891824996, + -38.929237385790294, + -38.60501927350662, + -38.26979751771763, + -37.9385310519133, + -37.61768400267112, + -37.277203470340076, + -36.93210187510192, + -36.5989670409393, + -36.2605671033598, + -35.917965476399594, + -35.58097928977165, + -35.25319314601881, + -34.92046457703637, + -34.60122646328586, + -34.29598638239527, + -33.98750709553549, + -33.68152469734872, + -33.39455183864643, + -33.105969452117364, + -32.81664183529315, + -32.54062425940163, + -32.26072655522608, + -31.99050060497156, + -31.72877191156155, + -31.468227199983055, + -31.21843403205665, + -30.977134869050094, + -30.744551200538588, + -30.520450877430214, + -30.306724802221535, + -30.101948932135542, + -29.90258558513699, + -29.711945872526478, + -29.52797590846506, + -29.350878727858255, + -29.18247502170537, + -29.01909000846517, + -28.861102358688207, + -28.708252894134002, + -28.557369426591453, + -28.409309883669305, + -28.268708922244162, + -28.126963558263956, + -27.984308901106722, + -27.85100925439488, + -27.71529717169798, + -27.576840023148723, + -27.44251871424342, + -27.313256895515355, + -27.180729339362657, + -27.04782848835487, + -26.92002153360609, + -26.790315116923516, + -26.660100898820513, + -26.53355776255497, + -26.405116036858658, + -26.276057964785224, + -26.148177020639906, + -26.02033937766196, + -25.88848134437824, + -25.755681789181292, + -25.624872901363535, + -25.490242505278097, + -25.355470932101024, + -25.22236384459584, + -25.088800442352564, + -24.95535322982862, + -24.82351898930916, + -24.689119954604983, + -24.558511466905593, + -24.425997464165782, + -24.290544088167824, + -24.157214348218936, + -24.021861518407203, + -23.885871920684064, + -23.7523139250111, + -23.619343365392247, + -23.486278389618853, + -23.35571588003516, + -23.227448808905987, + -23.09791383972981, + -22.973946373338865, + -22.853558225518288, + -22.732734754033817, + -22.615414309116364, + -22.50516554436355, + -22.397252798556945, + -22.29470452337946, + -22.194411644474002, + -22.095695812618867, + -22.005828463686132, + -21.918981677629155, + -21.832796787388066, + -21.748707256505124, + -21.6700995782513, + -21.588476863832106, + -21.502785620624614, + -21.42035927917382, + -21.33895745261171, + -21.25746798767443, + -21.173432789420303, + -21.093717056254025, + -21.01421380814389, + -20.92910815212903, + -20.845431538646466, + -20.763570632347665, + -20.683981750056073, + -20.605257257783588, + -20.527823423418333, + -20.450680999068584, + -20.37330676600147, + -20.297762528648224, + -20.22249251447037, + -20.149339684754093, + -20.07675386335212, + -20.001966877314885, + -19.930592511200402, + -19.85838426092376, + -19.784511919787423, + -19.71074907849777, + -19.641326428023383, + -19.57203711207824, + -19.499837444978393, + -19.419880274945527, + -19.3467494212837, + -19.270347377065946, + -19.18931053505767, + -19.105591213435552, + -19.01875416637844, + -18.92788485237338, + -18.8336186850511, + -18.73710595657461, + -18.634141549797043, + -18.525922295281873, + -18.413488281084675, + -18.296599974487968, + -18.171392414075658, + -18.040355925748102, + -17.910027118057833, + -17.77338992847017, + -17.629885914394755, + -17.487061658089928, + -17.339625102593942, + -17.18249906920041, + -17.027878626052093, + -16.873170781463866, + -16.706255793779288, + -16.540318732606554, + -16.376606673490276, + -16.2001022273604, + -16.023159396344724, + -15.850571060076042, + -15.664271270944365, + -15.475896287314427, + -15.298426651813514, + -15.091561042812065, + -14.892259901334407, + -14.705024033893125, + -14.512483072769651, + -14.305026564216309, + -14.10519411144605, + -13.903524572486049, + -13.692464657487488, + -13.483742809265246, + -13.274246955139253, + -13.059759920135956, + -12.84455403674611, + -12.627799045360609, + -12.409685667361273, + -12.1924413750488, + -11.970868912787585, + -11.7464236450359, + -11.522442826727415, + -11.295977029960582, + -11.067532313046188, + -10.839099112981014, + -10.61717452483189, + -10.388719073544767, + -10.16158941914945, + -9.937843429918663, + -9.708727087611281, + -9.480656932105369, + -9.253151528476407, + -9.026050056669934, + -8.799426062753268, + -8.577421694627647, + -8.356533192411291, + -8.134179831465978, + -7.912744714117008, + -7.694252414226817, + -7.476803140464561, + -7.259360915045362, + -7.045289914935967, + -6.835186557510802, + -6.6243444886204585, + -6.419611856274912, + -6.2210353157285665, + -6.02486892025979, + -5.83073140440032, + -5.640612261893859, + -5.4562844787822256, + -5.269901658356259, + -5.084882915776098, + -4.898135437540016, + -4.714547937466565, + -4.533587236559559, + -4.3288310694700245, + -4.143397569531199, + -3.967343203614429, + -3.7935401456925155, + -3.619734248375605, + -3.450697890446953, + -3.2925389530276097, + -3.1389235386498022, + -2.990376432907575, + -2.8535680191276174, + -2.7222266067770535, + -2.6005019413857395, + -2.497352417612733, + -2.4000035272737996, + -2.3089878530450045, + -2.2337449494523565, + -2.1701904299675587, + -2.1159681678033944, + -2.074625610913162, + -2.045475535555629, + -2.029161475575345, + -2.026358986910275, + -2.0371416488528715, + -2.0618610307693253, + -2.100938228557439, + -2.155907372425467, + -2.2255863659585926, + -2.3095800842277896, + -2.407399203609409, + -2.5225881586497594, + -2.6630509837274308, + -2.8185007212722084, + -2.986035983338265, + -3.183263172760066, + -3.397533865505796, + -3.619823420106108, + -3.8637291704136136, + -4.124021877019453, + -4.390613092861507, + -4.6773400070370545, + -4.975224485938064, + -5.27345518278105, + -5.587907304664719, + -5.911992859510572, + -6.230762785194832, + -6.55513967648047, + -6.886529345227692, + -7.206733306233928, + -7.5352635724096775, + -7.869206688399662, + -8.189496714942116, + -8.513422442809402, + -8.83988899464593, + -9.152968752423135, + -9.466355174354861, + -9.780120593229137, + -10.087283830882374, + -10.391203247701847, + -10.695466047400185, + -10.997952737530179, + -11.2968763513574, + -11.59695340044311, + -11.89762892723237, + -12.199047458639765, + -12.503552294087825, + -12.810250490063101, + -13.123043937553804, + -13.436433210200024, + -13.751874414867586, + -14.071036938215897, + -14.390312532211357, + -14.712179025001948, + -15.03737136375139, + -15.36352784876661, + -15.695121715030757, + -16.026625347948894, + -16.355412133457943, + -16.69674625962069, + -17.033640657897166, + -17.369267607448105, + -17.704054002405943, + -18.054108663868952, + -18.40259955279605, + -18.736412744667824, + -19.091415333251987, + -19.449470775356403, + -19.79053783368905, + -20.142884001997782, + -20.511361012524375, + -20.864014383527227, + -21.220138142465128, + -21.592449615714653, + -21.95105644301703, + -22.306083533920376, + -22.67429333385156, + -23.037392076997108, + -23.393195909901355, + -23.75368144445266, + -24.12053574396884, + -24.482355018737177, + -24.84277917128603, + -25.20546346341832, + -25.57296965523862, + -25.933788732782507, + -26.295052429160425, + -26.66664627437255, + -27.035480756971413, + -27.399127734923916, + -27.769060923278527, + -28.146037446226607, + -28.51169084023298, + -28.87468841939414, + -29.24624414963434, + -29.604713050167096, + -29.962627439354968, + -30.32445985346866, + -30.681273837093627, + -31.03950346961145, + -31.39679867260935, + -31.748219274989207, + -32.10410935222879, + -32.46541736589728, + -32.821335797797595, + -33.1796506882653, + -33.54110010763142, + -33.89853684918133, + -34.25586492273972, + -34.61612883746897, + -34.97040605630931, + -35.31793899655061, + -35.672835745619764, + -36.01812177167872, + -36.36670621473768, + -36.72506259706506, + -37.08436397400378, + -37.44385573591024, + -37.816975636283104, + -38.18916070593347, + -38.55455601119412, + -38.92460038130979, + -39.299503653654085, + -39.66411577470547, + -40.03225720112146, + -40.40549389662069, + -40.76736290191831, + -41.12269769746697, + -41.482852300709595, + -41.84225629509178, + -42.1938791629395, + -42.55609226584854, + -42.91934939165613, + -43.27528231397707, + -43.636636751629936, + -43.99890114014338, + -44.34786787571027, + -44.701227001123364, + -45.05899890206312, + -45.412497155596654, + -45.761181757254725, + -46.11438109125427, + -46.468353171673265, + -46.820220999222904, + -47.16840113637365, + -47.51786808464334, + -47.86888628608643, + -48.21737330898203, + -48.56310893062025, + -48.91521831222494, + -49.272464591852795, + -49.62173762377014, + -49.96970628222811, + -50.32443776675892, + -50.67498731428199, + -51.012205085006684, + -51.35865074123352, + -51.706623319588275, + -52.043596672557385, + -52.38645600411932, + -52.73627141639067, + -53.07957442147244, + -53.41971349747408, + -53.77095151892777, + -54.114957890414765, + -54.45019496610639, + -54.79856140679166, + -55.146660878574004, + -55.48081222853083, + -55.81990923461163, + -56.16331143241947, + -56.494210845184845, + -56.82777955894273, + -57.166836628774895, + -57.49731944828853, + -57.82638527590838, + -58.15808213867223, + -58.48476491883403, + -58.803442229655566, + -59.122058537933945, + -59.444892148310636, + -59.762422052013186, + -60.08008914024777, + -60.40578938497902, + -60.72188398200776, + -61.02658827196787, + -61.33811740634257, + -61.64817223693242, + -61.94016895889685, + -62.23021658249741, + -62.52097652742526, + -62.79630336970065, + -63.06227947815229, + -63.33040216818478, + -63.57961685109977, + -63.824155271857705, + -64.06078522539734, + -64.27601863921969, + -64.48611394439426, + -64.68291275551678, + -64.8688910556516, + -65.05883481493532, + -65.2295824985437, + -65.39107094542109, + -65.55888416912086, + -65.71953239347093, + -65.87235787735521, + -66.02122733852457, + -66.16097797595631, + -66.3001078504453, + -66.43607181753445, + -66.5649978184758, + -66.68447963809102, + -66.80496996462325, + -66.91749658058505, + -67.01499682792995, + -67.10451054468302, + -67.18398164698797, + -67.24855957610923, + -67.30529088716048, + -67.3489588700816, + -67.37678516450451, + -67.39878145401124, + -67.41350821822829, + -67.41462716887222, + -67.4014549685066, + -67.37738092142811, + -67.34327272135867, + -67.29916608466613, + -67.24443754844559, + -67.17819032906456, + -67.09504772459013, + -67.00028290392237, + -66.89428747725327, + -66.77724842495371, + -66.64867135313715, + -66.50926243576065, + -66.36805672868711, + -66.21853389972787, + -66.06400771932115, + -65.90164156157225, + -65.734632794016, + -65.56657721606501, + -65.39037351954278, + -65.21468008734657, + -65.03688644861485, + -64.85251657624295, + -64.66359622923198, + -64.47857172903966, + -64.28603113159828, + -64.08911541569569, + -63.89806627292407, + -63.702733993316485, + -63.50088321116002, + -63.29755695886102, + -63.09161931112981, + -62.87983162884179, + -62.6671544312839, + -62.451920772708206, + -62.2337393737835, + -62.0166704822151, + -61.792339473587255, + -61.56352323841773, + -61.33568055930336, + -61.1016579322237, + -60.862786281664306, + -60.62418752885767, + -60.383963344211935, + -60.141195225475855, + -59.901914270296935, + -59.660478242705985, + -59.42293859390448, + -59.18513332557597, + -58.941345177919416, + -58.7002959173493, + -58.46433951249894, + -58.22189301364764, + -57.986011878624765, + -57.75913529554494, + -57.52218633547478, + -57.29423931936483, + -57.0698705213643, + -56.837254235681065, + -56.611813590205564, + -56.388279414302474, + -56.15604413982848, + -55.93087161005591, + -55.71582916372175, + -55.48567875695876, + -55.254269686957365, + -55.03334941884798, + -54.80055829621406, + -54.56311277320943, + -54.33762906335555, + -54.10600016746531, + -53.869014087005894, + -53.63889286127345, + -53.40369522408548, + -53.16219284132628, + -52.931527826394884, + -52.703625976211065, + -52.47509795814298, + -52.25330743116507, + -52.03453983917092, + -51.82054456648979, + -51.602559678881605, + -51.384751277985615, + -51.162011385075836, + -50.93741702373425, + -50.71615399273452, + -50.48993478386764, + -50.25605295810436, + -50.021506529335404, + -49.788406338011605, + -49.54634219897052, + -49.30882398784405, + -49.078007785005084, + -48.843338943819056, + -48.61757268965913, + -48.39566728328335, + -48.160147679826615, + -47.91391012701571, + -47.681482808142384, + -47.445264365310344, + -47.19521380793525, + -46.94829331667921, + -46.705173200194736, + -46.45590249097712, + -46.19414798768018, + -45.938591625021985, + -45.69135464833386, + -45.436556691036294, + -45.1700352242094, + -44.9193464817555, + -44.66370427821693, + -44.38462162809643, + -44.10928850903636, + -43.84475633329836, + -43.56101912259667, + -43.26916780925776, + -43.00215538749537, + -42.730103248848714, + -42.43255454426534, + -42.14271369274558, + -41.85100933746453, + -41.53016148815435, + -41.210831215664115, + -40.88815509054743, + -40.5510344005366, + -40.21464673634862, + -39.873850051081675, + -39.521198215048955, + -39.17320252466675, + -38.81390435094139, + -38.45129600828928, + -38.0975338708504, + -37.73035353362482, + -37.35950643773611, + -36.99679419638933, + -36.632887522917756, + -36.26406605379358, + -35.91209689382184, + -35.565609465375076, + -35.21445744958684, + -34.89036309066244, + -34.57407088684302, + -34.24588188342661, + -33.94459584976613, + -33.647373976645035, + -33.34684672071522, + -33.0639932380401, + -32.78837738725645, + -32.5060827451208, + -32.233616178662146, + -31.974568655034858, + -31.715857675406586, + -31.461831254485197, + -31.22039968696241, + -30.987933228162344, + -30.760172487308616, + -30.53458248163382, + -30.314936479045173, + -30.104151305456135, + -29.899127412438165, + -29.697353012702013, + -29.5040090923272, + -29.31373973405169, + -29.123571697693556, + -28.941167377881264, + -28.76558256105084, + -28.587589482188562, + -28.420649669863987, + -28.264423190323043, + -28.103857859423396, + -27.95094691330261, + -27.807944638035227, + -27.660012091095293, + -27.515775067245535, + -27.37740325508359, + -27.236327458306935, + -27.09665204951298, + -26.959008262330904, + -26.818546998844113, + -26.67694543511828, + -26.53794803076415, + -26.39265680223, + -26.244917938631552, + -26.099260537815184, + -25.947248918197815, + -25.795964651986313, + -25.645215501942417, + -25.491078797437968, + -25.335977085982055, + -25.181077948821404, + -25.029432783995343, + -24.87271441809661, + -24.71553012384757, + -24.560659198297937, + -24.40528157733817, + -24.25222422438953, + -24.099913260358807, + -23.95199826779356, + -23.80705387831147, + -23.66389650129534, + -23.50860153926942, + -23.377205840182665, + -23.247375051793995, + -23.11994141914694, + -23.000168906362543, + -22.885095003156678, + -22.77561164838907, + -22.671021165335954, + -22.570048134501, + -22.475925129539053, + -22.3871268371147, + -22.297129381391294, + -22.208194495110913, + -22.124045343718752, + -22.032814062121165, + -21.938671705458578, + -21.84945388620275, + -21.75913901266592, + -21.66291869108665, + -21.571842556129557, + -21.47889433693878, + -21.377522783740005, + -21.279676525653205, + -21.187195966730886, + -21.092740410870768, + -21.00013433181636, + -20.910009128995, + -20.81803034627194, + -20.725254572870437, + -20.635908500377614, + -20.549458520919444, + -20.462560882591013, + -20.375027708946895, + -20.29179943034236, + -20.207735550713927, + -20.123140092350056, + -20.0422695909282, + -19.963284652474147, + -19.882417677939628, + -19.80006098860956, + -19.7164917979833, + -19.629668419272896, + -19.537748434389247, + -19.44329916286591, + -19.343888268147293, + -19.239331412049115, + -19.132778184945618, + -19.019851930988047, + -18.899693634244414, + -18.7740222361764, + -18.64098764588919, + -18.498694309792743, + -18.35070991187078, + -18.201133407197233, + -18.042508094909714, + -17.880072443923964, + -17.714131263705024, + -17.534272578123293, + -17.352049855298723, + -17.166474852006907, + -16.971031804739926, + -16.77285335055818, + -16.57108523614885, + -16.362789926429176, + -16.1568502058274, + -15.945924725414416, + -15.727011384518159, + -15.52087926321072, + -15.308608777976925, + -15.081023749659833, + -14.867190488523567, + -14.65261469516981, + -14.422873334341448, + -14.19775737997882, + -13.97538784029993, + -13.74029848216648, + -13.514067712925359, + -13.305662879590912, + -13.090133036188979, + -12.8749084736165, + -12.66460687077586, + -12.447283591667928, + -12.226123652789182, + -12.008635692779118, + -11.786103363661667, + -11.563554636491423, + -11.336508136336477, + -11.105139193387451, + -10.87310897973837, + -10.641936490991498, + -10.41504853696415, + -10.187361287970107, + -9.959962900483445, + -9.734518728442552, + -9.512464440594053, + -9.289723557559512, + -9.06962063046247, + -8.850349641018138, + -8.629493815200412, + -8.408026328287127, + -8.190105732462012, + -7.977462748886694, + -7.764403192292031, + -7.555317194741051, + -7.353277302203586, + -7.154073892218664, + -6.959369532699178, + -6.772135414139355, + -6.588362759891173, + -6.407471870227311, + -6.230809235740635, + -6.0514239953809925, + -5.873849213501359, + -5.704583332793421, + -5.517992193649563, + -5.351842156381033, + -5.195978546597032, + -5.0444182955658725, + -4.89452980585661, + -4.75265711854504, + -4.616714693316472, + -4.478987911620376, + -4.347586387492915, + -4.221261991556717, + -4.094105073197255, + -3.973620987203808, + -3.8582581195654524, + -3.743854038062321, + -3.6406250204203503, + -3.543159820531951, + -3.451697622539284, + -3.373083583306613, + -3.302958184860286, + -3.238454729469617, + -3.1840381544859695, + -3.138262606993281, + -3.101446047905101, + -3.075092558166955, + -3.057787655905119, + -3.051154313913769, + -3.057541909161938, + -3.0777058046686556, + -3.11226177300754, + -3.1633028148244224, + -3.2269528478740774, + -3.3055484962963204, + -3.411798257575102, + -3.5327781800075804, + -3.6598205132349197, + -3.8042761382970274, + -3.9637921100409432, + -4.1343610580237, + -4.321488710941954, + -4.5254183007263, + -4.743492198481461, + -4.976546490154955, + -5.217377413034586, + -5.463944656267498, + -5.725466210792774, + -5.9942265008430455, + -6.266880248401289, + -6.547174852306351, + -6.830449296321783, + -7.140692472535568, + -7.4244805791327595, + -7.712915267119155, + -7.99947758519533, + -8.287869958446791, + -8.580775449727524, + -8.870859696915527, + -9.158148441959966, + -9.44699351667771, + -9.732155623590316, + -10.014872063053648, + -10.29775979865664, + -10.5749857424261, + -10.852780150019186, + -11.13579070510385, + -11.414735024671282, + -11.691983571243979, + -12.001988427141596, + -12.284375625399804, + -12.56870016424334, + -12.853058122056988, + -13.137900260390332, + -13.42979842600842, + -13.719020086320706, + -14.009281632255217, + -14.301011028445044, + -14.592116426583539, + -14.890971598247424, + -15.184707720545834, + -15.481093409437367, + -15.785987358772202, + -16.092112434469538, + -16.39191902946486, + -16.69850670541692, + -17.0161434213426, + -17.32450689834855, + -17.639360481835368, + -17.952300054487637, + -18.27492426202884, + -18.60534648107577, + -18.923198927697737, + -19.25420879463818, + -19.595659059684134, + -19.922765883100173, + -20.253464392487235, + -20.601756971249607, + -20.942552896658825, + -21.278023387366545, + -21.630659974387093, + -21.981249616001126, + -22.32017323749989, + -22.668377046161826, + -23.05463993470257, + -23.399669839059996, + -23.744378497142918, + -24.093191328550297, + -24.44436936299327, + -24.795515704871704, + -25.14414383087788, + -25.496241874938153, + -25.849278367345228, + -26.20207562252542, + -26.55782194511243, + -26.9148884183778, + -27.273738204083404, + -27.62996812180062, + -27.988628487833694, + -28.343905979595174, + -28.69105165873182, + -29.037582666102935, + -29.38635666555043, + -29.723594788931088, + -30.056689528810995, + -30.398340778637635, + -30.732889562974144, + -31.064289829857046, + -31.399693377691765, + -31.73114349509177, + -32.06073470265107, + -32.39085993270465, + -32.726477933935236, + -33.06476388178211, + -33.39372387206702, + -33.72672082897999, + -34.065249854126584, + -34.39760966485348, + -34.73058887095751, + -35.06296182746248, + -35.38255055373931, + -35.70814463524432, + -36.039025349592656, + -36.35972932359205, + -36.68348309060314, + -37.015753113562184, + -37.34542599091974, + -37.67911904288027, + -38.020693570149206, + -38.36066205243361, + -38.70234003793076, + -39.0451492945628, + -39.39470491993342, + -39.74042583403011, + -40.08281748005475, + -40.43186199005066, + -40.78057224431363, + -41.12203865389018, + -41.46267864179745, + -41.808416009280755, + -42.1509228039912, + -42.49332936056103, + -42.8435430598219, + -43.18692325830212, + -43.52734250106671, + -43.8776940098036, + -44.21698436032345, + -44.556835639299806, + -44.89983918658318, + -45.242047277374795, + -45.5826423524521, + -45.92626112623457, + -46.26649535067602, + -46.60896010384153, + -46.949825362626704, + -47.28504508563118, + -47.62134759487119, + -47.95862599059289, + -48.29311570298762, + -48.62734834939975, + -48.96608905881075, + -49.30823556860031, + -49.64709563386687, + -49.98457430475821, + -50.32742155888007, + -50.66680723085256, + -50.992571879080906, + -51.32806674987615, + -51.6678732874057, + -51.99491914646086, + -52.33010756345978, + -52.67357247262765, + -53.010980957968805, + -53.346069446378266, + -53.693569866543896, + -54.037595073463436, + -54.373086426955794, + -54.72237993359764, + -55.07397645239609, + -55.412614130024785, + -55.75677926690112, + -56.106190350043306, + -56.43909434686085, + -56.77630047366981, + -57.11950997288579, + -57.455225540470664, + -57.78897197103224, + -58.126435303607316, + -58.45929014889902, + -58.78439546632375, + -59.11397571401777, + -59.45000916190445, + -59.77746170812227, + -60.108321017355244, + -60.440039735420875, + -60.751897208358145, + -61.05539281208393, + -61.35354119299772, + -61.643599581934104, + -61.912007900310435, + -62.18193003148963, + -62.44967261244594, + -62.69734117106019, + -62.94625825298737, + -63.1885117669288, + -63.44629136317529, + -63.71321495812481, + -63.96855143572826, + -64.21750078899412, + -64.45998084573628, + -64.69048974510206, + -64.9097276652017, + -65.13231715848787, + -65.33119631774655, + -65.52088910011457, + -65.70062746247886, + -65.8565819388565, + -66.00708425595732, + -66.15325504641662, + -66.28385190357783, + -66.40717356075399, + -66.52631527291177, + -66.62958093381904, + -66.72924851871147, + -66.82707187290991, + -66.91117047812757, + -66.98971235880302, + -67.06456025223856, + -67.1300129744069, + -67.1889735503678, + -67.23783511245693, + -67.27488175592373, + -67.30723066911459, + -67.32866615617212, + -67.33249111606224, + -67.32121765826327, + -67.29769196841443, + -67.26372753216401, + -67.21770276411456, + -67.15775947255702, + -67.08362594093559, + -66.99374450078747, + -66.88971886776457, + -66.7747525958143, + -66.64499872374087, + -66.5047908956285, + -66.35542672806882, + -66.19544669942078, + -66.02707019249198, + -65.8507498384762, + -65.66671210846613, + -65.4831662845677, + -65.29471575998292, + -65.09597657589894, + -64.90250498287553, + -64.70670321752343, + -64.49651569008526, + -64.28647349591253, + -64.0832888223937, + -63.8636405810657, + -63.6391231646599, + -63.4189590613997, + -63.19149285289527, + -62.95389538462015, + -62.71839446630718, + -62.478440369530034, + -62.23445317242666, + -61.98963337497661, + -61.740036658029396, + -61.49076695590796, + -61.241328126844905, + -60.98683623445296, + -60.73310879462256, + -60.477894447425896, + -60.220686607194, + -59.96636730543988, + -59.714213309905475, + -59.4590148414393, + -59.20951404769694, + -58.96006911126184, + -58.70508896088304, + -58.45649206354154, + -58.20666486943769, + -57.951979833842046, + -57.70589026616218, + -57.464689786537754, + -57.21669082657974, + -56.980934676128975, + -56.745921029818305, + -56.50802576386077, + -56.276750829476114, + -56.04317179109562, + -55.80867142570943, + -55.57667262122025, + -55.341456448501326, + -55.10434011192988, + -54.877842723637585, + -54.643498335668696, + -54.403127624811034, + -54.168984336643426, + -53.93372568497106, + -53.69061024371152, + -53.45504493834885, + -53.21811788454279, + -52.972977071567044, + -52.72832673016044, + -52.47843122474741, + -52.22517971900285, + -51.97870037637053, + -51.73492669184909, + -51.495159189060026, + -51.25994896995887, + -51.02679480745267, + -50.7949423064563, + -50.56273688923523, + -50.322646298805935, + -50.0518369270235, + -49.80940060456466, + -49.56274912715791, + -49.30633051674411, + -49.048153248661066, + -48.788574214159524, + -48.528002933079165, + -48.26586732939847, + -48.0063101472052, + -47.76030630870287, + -47.52441189578046, + -47.291128619127235, + -47.05883153043223, + -46.82799772148867, + -46.59482025492095, + -46.365585376495105, + -46.11109902775143, + -45.87727287335853, + -45.642914140177105, + -45.40265378491914, + -45.152736261013686, + -44.897224738899496, + -44.65296719519171, + -44.40939803456398, + -44.15264580641331, + -43.90177598054978, + -43.663467940761436, + -43.409574537718164, + -43.149188443721904, + -42.91191714832933, + -42.672110917469624, + -42.423003772869414, + -42.17677983247791, + -41.929123357104196, + -41.67955502117077, + -41.413873554753664, + -41.14334513864083, + -40.86956510701395, + -40.5741653030368, + -40.26044947653966, + -39.94043634888155, + -39.610520471411476, + -39.277888904030334, + -38.93790784879036, + -38.59248930617415, + -38.2485112170478, + -37.89317134392239, + -37.53036248814005, + -37.174427806825406, + -36.85794224453516, + -36.53845093440649, + -36.21348196170747, + -35.89285402641411, + -35.582396884716175, + -35.260874017508556, + -34.95264533947338, + -34.657933152357934, + -34.364936308225865, + -34.06839631466887, + -33.75798183397996, + -33.453822376036925, + -33.15327496730229, + -32.85245477930143, + -32.55264967900641, + -32.25956613718065, + -31.968360034488466, + -31.678558658588116, + -31.395253608918217, + -31.12487086303531, + -30.86073486042292, + -30.596911207466043, + -30.35094700172187, + -30.107916916846243, + -29.86662969238843, + -29.64140287514268, + -29.42465730060835, + -29.20873141950385, + -29.003208174876242, + -28.801047166518092, + -28.598559421673098, + -28.408279489559504, + -28.226271545182716, + -28.04055186945441, + -27.87096283596155, + -27.71148765551883, + -27.546167521352587, + -27.39230417727304, + -27.24877580876583, + -27.09783215893978, + -26.954937921799782, + -26.818949965758552, + -26.681676601821323, + -26.547398310076627, + -26.416396387906435, + -26.285743836643963, + -26.154399525411, + -26.028216440474537, + -25.89718916840326, + -25.76383098390717, + -25.63069841007004, + -25.496061982526225, + -25.359107458747367, + -25.224247495385317, + -25.089574969004996, + -24.95285873066305, + -24.819494799318345, + -24.684718566617278, + -24.554116616503265, + -24.419019357854157, + -24.284172149282224, + -24.148707793931205, + -24.01186473981563, + -23.876342746496064, + -23.738868073052906, + -23.601694397583806, + -23.46532239616867, + -23.32692643656262, + -23.18872086142345, + -23.05105840492789, + -22.91885948279839, + -22.785663621276885, + -22.654800919142524, + -22.532057954698, + -22.411929968003665, + -22.29671530099123, + -22.186545449912806, + -22.07959033186948, + -21.979140273166, + -21.88559955764864, + -21.793155405157744, + -21.701638186393176, + -21.617775345367917, + -21.53450567338066, + -21.44536929480654, + -21.36241397007175, + -21.284528674198466, + -21.205495079814423, + -21.127556831766018, + -21.054384220246313, + -20.97955361232512, + -20.90073577389424, + -20.82357507949722, + -20.74731866628342, + -20.6711798956272, + -20.593662158870323, + -20.515250902662043, + -20.435299164524103, + -20.35293541021248, + -20.268813027999187, + -20.185173336563242, + -20.10105356746033, + -20.0118211480889, + -19.92464043858409, + -19.836792297259407, + -19.747523545570328, + -19.6571131930019, + -19.570821403242952, + -19.486117255630514, + -19.40006239938822, + -19.31576568805221, + -19.230069952903875, + -19.143307897505597, + -19.054961879789328, + -18.965652637571274, + -18.873872155913173, + -18.778858428906904, + -18.68578786711409, + -18.58738025704817, + -18.483825961339633, + -18.381112187414274, + -18.2740657788208, + -18.162325042151902, + -18.04407412307408, + -17.919854667959843, + -17.792509316116345, + -17.66184175675464, + -17.523915424889008, + -17.380889757343244, + -17.23131569457251, + -17.07543853247107, + -16.912602931612362, + -16.742816293028714, + -16.57246049417901, + -16.3943147157906, + -16.212966686173175, + -16.032686508658802, + -15.847141521277802, + -15.651425656594203, + -15.464992511939615, + -15.278464908435609, + -15.072448379880425, + -14.8761217062947, + -14.685520947858995, + -14.480019650318107, + -14.27821657976232, + -14.081162336099894, + -13.878548719850825, + -13.676114825254635, + -13.479426329362635, + -13.281653292041767, + -13.081932356207679, + -12.883715506200103, + -12.679829250555638, + -12.474074208665568, + -12.264738759020902, + -12.052432689037936, + -11.836503537782825, + -11.621061684740479, + -11.401713923807238, + -11.180083391260311, + -10.958317772071153, + -10.731046098698549, + -10.505790343847037, + -10.278488349239959, + -10.047943367741272, + -9.817561506108019, + -9.587572898564792, + -9.35766954806178, + -9.126937303434898, + -8.900387525863927, + -8.67631977764867, + -8.448014145550228, + -8.220877478765015, + -7.998791010097954, + -7.773894684448212, + -7.541617244165808, + -7.320168571875958, + -7.098763267260314, + -6.8729716888569605, + -6.652353600687759, + -6.43479038495738, + -6.217797107537136, + -5.998216552350932, + -5.776678779729985, + -5.560223461091686, + -5.345274024557623, + -5.127561761243892, + -4.917491843645876, + -4.715461570545477, + -4.505065395882604, + -4.307361113708217, + -4.124523779496649, + -3.9355579397707596, + -3.7491507672574134, + -3.5787963356111887, + -3.406589393198464, + -3.24175832452202, + -3.086619820093648, + -2.930549832146983, + -2.795652688582039, + -2.6676514831264004, + -2.5401125480067064, + -2.4299029546326705, + -2.333505659562151, + -2.2450534950716903, + -2.17204735922884, + -2.1128176477362017, + -2.064530519347138, + -2.030370074393183, + -2.010393138338829, + -2.004138709737753, + -2.012870078493015, + -2.0370841526207615, + -2.0763602872947198, + -2.131686324194107, + -2.2055013778985, + -2.2978840990695226, + -2.4069163471706303, + -2.5364879960175424, + -2.68534378683897, + -2.848188644199925, + -3.037611048174019, + -3.2441099854616016, + -3.4647746970836804, + -3.703799504275772, + -3.9553835254678953, + -4.218520760246446, + -4.495366437253349, + -4.785639833248068, + -5.08484574406934, + -5.386986180057311, + -5.702574899155793, + -6.024555013090089, + -6.348233572480257, + -6.676596928656058, + -7.002112491822304, + -7.323535097965916, + -7.64907641442559, + -7.974162122243784, + -8.29329386687016, + -8.614724092092834, + -8.967351104418675, + -9.281004572916306, + -9.595351486147798, + -9.906920463817347, + -10.215609104682294, + -10.522452460283825, + -10.822021762894998, + -11.12167338367002, + -11.419731886306183, + -11.71402587006081, + -12.009303977815186, + -12.3039125702886, + -12.59794267786584, + -12.890491044518715, + -13.183914504029614, + -13.481320566748892, + -13.77774003043691, + -14.076319692672689, + -15.135851789820482, + -15.448746547073267, + -15.7701082496401, + -16.09652425161251, + -16.419857209306667, + -16.74923535317632, + -17.087754028882582, + -17.418509385474984, + -17.749829044650806, + -18.081905985797718, + -18.43035359220856, + -18.774654053643356, + -19.111139682495992, + -19.46983396195551, + -19.826603384066924, + -20.169733909783766, + -20.52679978792883, + -20.892862525450653, + -21.24183538689616, + -21.599351761140987, + -21.969785438463287, + -22.3253797601872, + -22.684291974552025, + -23.052360362425834, + -23.41196124967644, + -23.770153047626177, + -24.133781319508433, + -24.50390460074309, + -24.872779437632673, + -25.240713528172346, + -25.613006170135684, + -25.986125133400538, + -26.356572373107657, + -26.732719884490372, + -27.110898722633916, + -27.488921639221125, + -27.867802714125826, + -28.250522161210075, + -28.627673559074477, + -28.999630763154563, + -29.374260600789675, + -29.747133847477823, + -30.108627139695123, + -30.4738748947489, + -30.845345249687618, + -31.20496502543251, + -31.56838718343814, + -31.934373093137385, + -32.29625825293812, + -32.655795528669486, + -33.023786562502075, + -33.394066451795176, + -33.75437144700971, + -34.12279959224543, + -34.49565203273659, + -34.86116745868966, + -35.22769623168896, + -35.58748552490108, + -35.93626152603457, + -36.29294327802148, + -36.64582261485386, + -36.995657073893845, + -37.35707992209258, + -37.719757995762386, + -38.08279784436257, + -38.45545006209987, + -38.82825938742799, + -39.19826192510871, + -39.57148727374065, + -39.94837305387175, + -40.3131466176835, + -40.68148435156973, + -41.053580633180246, + -41.41471059380327, + -41.771110999942785, + -42.13160233797795, + -42.48929319127417, + -42.83258211303702, + -43.17156392946248, + -43.50777876759926, + -43.83621764122727, + -44.17080271291086, + -44.52739005309362, + -44.84879118581973, + -45.172689735288515, + -45.49298321643188, + -45.81118501611773, + -46.13863714923011, + -46.46773403246215, + -46.795166887960754, + -47.1257062847864, + -47.44813465160126, + -47.76764993598219, + -48.086771459399884, + -48.40222941032584, + -48.71907808757432, + -49.03399471655375, + -49.34354732591616, + -49.64813474894097, + -49.94981490289839, + -50.251862128363726, + -50.55493943284664, + -50.85796949254924, + -51.15180674861884, + -51.44529451950592, + -51.74712457823629, + -52.04292433028553, + -52.33742608542614, + -52.64444297727034, + -52.95310408136931, + -53.25735769920424, + -53.559259930307086, + -53.868186045975776, + -54.176431125540695, + -54.478829869137414, + -54.78500535792089, + -55.095673466036885, + -55.40471352965616, + -55.714435944882986, + -56.02605195190667, + -56.33613734445318, + -56.64287796102448, + -56.94404246659396, + -57.2454612627552, + -57.547812439847576, + -57.84562457342142, + -58.13879443261982, + -58.433761921080084, + -58.72899131468643, + -59.01665140201153, + -59.30114472694361, + -59.590252232730855, + -59.87847927387634, + -60.162801188955875, + -60.44947893580497, + -60.738067632631044, + -61.01861847533337, + -61.29090609295296, + -61.55798266103622, + -61.823011438212895, + -62.08601868209725, + -62.332524119480766, + -62.585320398106404, + -62.835515300168986, + -63.07422177960659, + -63.31370164822563, + -63.54605428117169, + -63.7728317320232, + -63.998742709320645, + -64.2148619436213, + -64.4281100223949, + -64.63494551952286, + -64.8312054221813, + -65.02270635297941, + -65.21286313376582, + -65.38030932523728, + -65.55025723287066, + -65.7186392970733, + -65.87307562484358, + -66.02658267148043, + -66.16997682946553, + -66.30036537860236, + -66.42860486157353, + -66.54586257112908, + -66.65080224457779, + -66.75524537867345, + -66.84978197627228, + -66.93204206142951, + -67.00986209234982, + -67.07818786683039, + -67.13766003323333, + -67.18855777233131, + -67.22686182700441, + -67.2561763081435, + -67.28037100105091, + -67.29232540801087, + -67.28902093918421, + -67.27309618532365, + -67.24619829938165, + -67.20977023756896, + -67.16182994888243, + -67.10232748696933, + -67.0303655676146, + -66.94613023023517, + -66.85302319135522, + -66.75040284348367, + -66.63711735946971, + -66.514289986404, + -66.38405686478657, + -66.24356107069106, + -66.09319796269615, + -65.93253183883319, + -65.76682167290186, + -65.60006941227343, + -65.42529806307405, + -65.24472705747665, + -65.06968890862828, + -64.88883274173362, + -64.69673994010479, + -64.50796366224124, + -64.32312159258277, + -64.12314018644014, + -63.92081827337502, + -63.72362476919724, + -63.518495215160286, + -63.30621814697917, + -63.09775156198335, + -62.88512388334975, + -62.66755489876957, + -62.45221622175098, + -62.23244384909481, + -62.01294574095953, + -61.797165848834396, + -61.57844859793764, + -61.35676750292627, + -61.13764904108875, + -60.913958948083966, + -60.68835770839192, + -60.4639032078944, + -60.240140380911946, + -60.01428124793145, + -59.79349400741151, + -59.568453565516144, + -59.34337280034526, + -59.12099930516383, + -58.89234417074142, + -58.66546206150973, + -58.443926654764425, + -58.22197563076419, + -57.99691914914282, + -57.7858573920524, + -57.56798673677686, + -57.347636867085846, + -57.13459913937889, + -56.912562430064085, + -56.687429448579756, + -56.468789942761816, + -56.245386985762075, + -56.018754601727956, + -55.799065418774035, + -55.5855183332723, + -55.36229348495816, + -55.14053947443615, + -54.9222949185408, + -54.70282645857301, + -54.480597882969064, + -54.26499998226399, + -54.05146625314503, + -53.833881587790664, + -53.61799894792032, + -53.40159086033317, + -53.180210132692544, + -52.962174855186966, + -52.747858101984505, + -52.5351623575907, + -52.328901860940086, + -52.12870463886185, + -51.924782676423575, + -51.72756685644262, + -51.53106038072766, + -51.3286602838826, + -51.123117717595456, + -50.91323763770998, + -50.70543784235384, + -50.49438155851113, + -50.27510822028462, + -50.050909480063616, + -49.823669528226226, + -49.59422691319602, + -49.36440612388891, + -49.134583903761175, + -48.90276059387876, + -48.6776976551374, + -48.45875150493637, + -48.237037244812875, + -48.01299505200677, + -47.78735522324146, + -47.56730557931867, + -47.34259502952261, + -47.11120550652267, + -46.88007594379417, + -46.64671633762666, + -46.416150078136745, + -46.172300421953395, + -45.92810876478263, + -45.68903856733507, + -45.446385626242474, + -45.18985324628705, + -44.94372691322733, + -44.70192897308499, + -44.4388573852421, + -44.17362366263393, + -43.92040723303416, + -43.65516753542635, + -43.37830386078054, + -43.12025807725942, + -42.86759601270207, + -42.58731737506445, + -42.30107905713094, + -42.022423625323306, + -41.719980538226665, + -41.41304072763449, + -41.11286088896048, + -40.795923494360935, + -40.48217486048098, + -40.16624475184209, + -39.83954074672723, + -39.51433138503104, + -39.19059499041351, + -38.855692630079304, + -38.52375773208069, + -38.19461421504617, + -37.84480949933679, + -37.49163018486492, + -37.144637803730156, + -36.7925621307796, + -36.43533341187442, + -36.086511974681386, + -35.74411094330811, + -35.3974239198621, + -35.070615321269564, + -34.75612404371996, + -34.433828009503436, + -34.119296782661245, + -33.82917333821225, + -33.53119170654072, + -33.23896565819366, + -32.96298573650396, + -32.68880755570047, + -32.411180307255314, + -32.14730907317692, + -31.89148736574123, + -31.63406399894263, + -31.382816104159236, + -31.14260470511524, + -30.908738528997826, + -30.679175267360588, + -30.453972229323984, + -30.239954435773935, + -30.034953451987597, + -29.83544699147934, + -29.63870010592736, + -29.45051392019687, + -29.26484764906907, + -29.080832426982415, + -28.904547605644584, + -28.733812127457593, + -28.560948097803262, + -28.398201418249936, + -28.244219460857213, + -28.085503298428566, + -27.93239840355711, + -27.788020578850716, + -27.6382945306535, + -27.489944406321268, + -27.347831741300396, + -27.204421570626206, + -27.05296883779879, + -26.898753759572028, + -26.743990246723495, + -26.58693748758553, + -26.434903402143977, + -26.27864859898281, + -26.118987136725572, + -25.960688848867633, + -25.80263384082788, + -25.641587531545945, + -25.490634425886167, + -25.345428899364343, + -25.198233210221513, + -25.054022176188564, + -24.90948308823688, + -24.768150125688518, + -24.622367149289303, + -24.47454313616094, + -24.32689585944346, + -24.177551516639706, + -24.02368924149357, + -23.864973243607917, + -23.70908548011217, + -23.55665080355543, + -23.40380073678331, + -23.255790164156334, + -23.112005237974362, + -22.974921510501254, + -22.836658156324113, + -22.701933896202043, + -22.57860038025221, + -22.460292385077597, + -22.34604813811691, + -22.235431708965255, + -22.12778442554132, + -22.02883321507727, + -21.934526113250715, + -21.841440296149116, + -21.750985679990453, + -21.667079661963893, + -21.5867976110394, + -21.508110597429436, + -21.437055558110558, + -21.36902699900956, + -21.298175186937353, + -21.230834669316433, + -21.167569360273735, + -21.099309576083893, + -21.030590119263348, + -20.963200368387593, + -20.890956437634305, + -20.8155983053334, + -20.738588165632944, + -20.660873903780818, + -20.58267998099796, + -20.502745508009546, + -20.421955384431214, + -20.34330220580916, + -20.265145814853938, + -20.18118020561016, + -20.102040809730145, + -20.026286139676, + -19.94960758496676, + -19.87154028788423, + -19.796544460529947, + -19.722975346652813, + -19.646816421842942, + -19.568828146982252, + -19.485389574699816, + -19.396748886114946, + -19.300280064223347, + -19.196228108034806, + -19.088681697785233, + -18.973792366598712, + -18.855078107823672, + -18.73464300770831, + -18.60742735561582, + -18.47426458226979, + -18.3356953708922, + -18.192468238461107, + -18.05014620355314, + -17.900311344850667, + -17.748259627428386, + -17.59985239845478, + -17.44749018124793, + -17.289831861687734, + -17.13476711030833, + -16.978987429061053, + -16.815182750249186, + -16.649125894399738, + -16.484893876493732, + -16.31141786509426, + -16.13937011537734, + -15.966794840397359, + -15.78756663352844, + -15.60156817702558, + -15.42305278717738, + -15.242249598749698, + -15.043921348952347, + -14.860129157670553, + -14.670611824815383, + -14.465757156182125, + -14.264695572567078, + -14.063136254561105, + -13.855595937981844, + -13.646791011708268, + -13.442235373770762, + -13.233586255091215, + -13.022735948499761, + -12.812340887866805, + -12.599936269082722, + -12.3875157182114, + -12.174185056595656, + -11.961061223735184, + -11.743969719285408, + -11.530653431910961, + -11.314437774434387, + -11.10220201621013, + -10.891674418989599, + -10.678543557240541, + -10.469547248740419, + -10.2624515237825, + -10.051378919587437, + -9.841779070441515, + -9.634490779696469, + -9.427093585360735, + -9.218670150847851, + -9.015585082397052, + -8.813825543884338, + -8.607017514119331, + -8.401703878797997, + -8.20298074403046, + -8.004702741756736, + -7.801652168700154, + -7.610108952407171, + -7.42142288803849, + -7.2264885692473735, + -7.039059526496087, + -6.8538488835666485, + -6.667048806111803, + -6.480069200058203, + -6.290853721787352, + -6.098503309303771, + -5.9105502171822195, + -5.7192761035844, + -5.529391050315129, + -5.346962338968414, + -5.156469172134299, + -4.973915853484037, + -4.797752869141554, + -4.6190817736512875, + -4.447224680955508, + -4.283957503658762, + -4.125194422673401, + -3.970335520706842, + -3.823632292155835, + -3.6807675216413864, + -3.547546651900776, + -3.423187895815954, + -3.3027916263417385, + -3.192411461611369, + -3.091623219459845, + -2.9974353690905984, + -2.9142283907573603, + -2.842398311146486, + -2.78200160182335, + -2.7332929856987773, + -2.697585945913979, + -2.6758406562339077, + -2.6692647170003116, + -2.6794013408390134, + -2.7068345307342736, + -2.750243888382191, + -2.8140204735141774, + -2.9019989163318414, + -3.0102149280354915, + -3.136163434944975, + -3.292116164523131, + -3.4732407107632817, + -3.6673367794577665, + -3.8855819480982174, + -4.121239444135406, + -4.367886537153666, + -4.6310590045092255, + -4.909107602324732, + -5.198241707007494, + -5.493338337837971, + -5.801065701760351, + -6.11715304648553, + -6.436004558689778, + -6.761900661809447, + -7.08484882685639, + -7.406039134764869, + -7.729457535360628, + -8.053267053009991, + -8.37159367129153, + -8.68936576889038, + -9.004949568297512, + -9.31682047245802, + -9.62696774417688, + -9.93245417582924, + -10.235487151275668, + -10.536757563008043, + -10.831021603735033, + -11.126865921930344, + -11.422013064881448, + -11.711720823201341, + -12.003050423904435, + -12.296803887576305, + -12.589869543335947, + -12.882519007259162, + -13.206824304862469, + -13.506153041046147, + -13.804482252710493, + -14.104526524025465, + -14.407232207829418, + -14.713380061745976, + -15.025173389468257, + -15.339392391657894, + -15.657636340223394, + -15.985835400864142, + -16.315854623638366, + -16.645196662538208, + -16.98721157759249, + -17.32813326129069, + -17.670578183215763, + -18.012771106910797, + -18.372457112967986, + -18.728236276014083, + -19.07590688798593, + -19.448794833792853, + -19.819856423742173, + -20.17754088957126, + -20.55025241238078, + -20.93166781440896, + -21.29683990132369, + -21.674990368027238, + -22.06002927686083, + -22.43131262677279, + -22.81070485571307, + -23.193418361000578, + -23.565429945496376, + -23.940763282180978, + -24.326808498268797, + -24.70813635926141, + -25.085573837306793, + -25.471415865390853, + -25.85905754259594, + -26.24067967699594, + -26.62835577756132, + -27.023276222597836, + -27.41673543567052, + -27.810942640593055, + -28.21092489010343, + -28.603232087112556, + -28.99032235059943, + -29.38285130277187, + -29.7640527462422, + -30.138842298441205, + -30.524940716961034, + -30.9031609071591, + -31.275535933580933, + -31.653674064251156, + -32.031825364270404, + -32.40078807806816, + -32.77565139090284, + -33.15572092407154, + -33.524616131218615, + -33.89788555388855, + -34.31205031475888, + -34.67917302969318, + -35.047490717617805, + -35.40843109635998, + -35.765511114477306, + -36.12346065001754, + -36.47838098507164, + -36.83849735418127, + -37.20493953280114, + -37.566328907076304, + -37.93701170042838, + -38.31494730759715, + -38.6832600789177, + -39.04813864890323, + -39.42307988909404, + -39.788370226073305, + -40.14692481413972, + -40.514965166964416, + -40.876722858299054, + -41.22342213010617, + -41.574186828760354, + -41.93368443234105, + -42.28015563616246, + -42.63314166434982, + -42.98871329167694, + -43.336115226771106, + -43.682257706749766, + -44.03144549426208, + -44.37217780132779, + -44.714603335792184, + -45.056444297411836, + -45.39785578436033, + -45.73739926610399, + -46.07722256707363, + -46.41392265976048, + -46.7536884124984, + -47.08837036597805, + -47.41816538407625, + -47.74972236062895, + -48.07810458489749, + -48.406582657969295, + -48.735465316936306, + -49.06695767124381, + -49.39895970424358, + -49.73018564900906, + -50.06126296328641, + -50.39551242883402, + -50.72834905185715, + -51.05150893735426, + -51.38114781399141, + -51.717259812594975, + -52.07301299189745, + -52.39875053442496, + -52.733819938449834, + -53.06355532999838, + -53.38589277702482, + -53.716245957876666, + -54.04869301317649, + -54.37178924855617, + -54.69642329062652, + -55.02574949580002, + -55.34834129208912, + -55.669145147422846, + -55.98960028704136, + -56.3076972647037, + -56.621269058949764, + -56.93013001812104, + -57.24022509237409, + -57.55092377293988, + -57.85341381392597, + -58.15499335547893, + -58.49317394099913, + -58.795631088408726, + -59.091024596135846, + -59.39217311527495, + -59.69419299884416, + -59.99299964095694, + -60.29196990099787, + -60.59386658038928, + -60.88848406793987, + -61.17521208756635, + -61.4546765742016, + -61.735882797629706, + -62.00895634729002, + -62.26878311208972, + -62.535928099095486, + -62.79467599850539, + -63.04577366907901, + -63.29258615431078, + -63.532465026755965, + -63.76603160470231, + -64.01143698884582, + -64.2470503431708, + -64.47776937000921, + -64.69985146036979, + -64.91011890076263, + -65.11988179054424, + -65.3067541269102, + -65.49349294740588, + -65.67787352151993, + -65.84831801820904, + -66.00543237013562, + -66.15149592500164, + -66.28718826595971, + -66.42162106870808, + -66.5449400884666, + -66.65912764232849, + -66.77431064903143, + -66.87844686027412, + -66.97074860087946, + -67.0581201025096, + -67.13291211280782, + -67.19976263984901, + -67.2577201728544, + -67.30180824980346, + -67.33745312389016, + -67.36778895840271, + -67.38511135378526, + -67.38654074476754, + -67.37602965781693, + -67.35486159496755, + -67.32253305632219, + -67.27822350802018, + -67.2214423833344, + -67.15138090897804, + -67.06713408138613, + -66.97115496015253, + -66.86398241166371, + -66.74260535983153, + -66.6124286904359, + -66.47603631568566, + -66.3223538863303, + -66.16347486178844, + -66.00119848845848, + -65.83522823405262, + -65.67165984607001, + -65.50577920561427, + -65.32982397618471, + -65.15726721485673, + -64.9839874745163, + -64.80122020292747, + -64.61961921010575, + -64.43955916572844, + -64.25791070889484, + -64.06429147947685, + -63.86949330469262, + -63.67349152816083, + -63.47230699031968, + -63.26594689905641, + -63.0585791504768, + -62.84804847784432, + -62.63558106177187, + -62.42359538938827, + -62.20750638047554, + -61.99064999614591, + -61.77627137593541, + -61.5591030115546, + -61.3393009777562, + -61.119650829716875, + -60.895726935657365, + -60.668349275660866, + -60.440341380750475, + -60.21114195590213, + -59.9799352263799, + -59.72910276464591, + -59.498188999257664, + -59.26975687989393, + -59.04510407064621, + -58.818231079100684, + -58.5988116024957, + -58.38759521545623, + -58.175723036610236, + -57.967848630035796, + -57.77784702640796, + -57.582916018358446, + -57.38763405398029, + -57.2043625573086, + -57.01231722463997, + -56.81442331127769, + -56.62234054540864, + -56.42458477237226, + -56.224255895006415, + -56.02328811809464, + -55.81992462558698, + -55.62008498502751, + -55.41666243639262, + -55.203566020502805, + -54.99347717623862, + -54.7827257372063, + -54.563907961629525, + -54.34541200182019, + -54.12995911009073, + -53.90694176591345, + -53.68048133160258, + -53.45608410949461, + -53.22444569531539, + -52.988828306748424, + -52.759969627072124, + -52.5307230438456, + -52.30249765484058, + -52.08318313925874, + -51.8617495682587, + -51.64291895230069, + -51.42674271864286, + -51.205374836573014, + -50.978822865355845, + -50.74893558123603, + -50.520451718492126, + -50.286235222182874, + -50.04404205777941, + -49.79841125314252, + -49.55133029598883, + -49.301142538041006, + -49.05171706351374, + -48.80262570546493, + -48.55795012537514, + -48.320503554169306, + -48.08122853844462, + -47.83923244013082, + -47.597854027344184, + -47.36296859969274, + -47.119848164271716, + -46.87179152215019, + -46.62784398461014, + -46.3865185398944, + -46.13758662508226, + -45.882506913206186, + -45.638350768579734, + -45.40202584931485, + -45.15286767081181, + -44.910602981557574, + -44.68683528605533, + -44.44814233486934, + -44.197869411745884, + -43.958137197320205, + -43.7175201928318, + -43.4659316411832, + -43.20799095845028, + -42.958669988965056, + -42.714975562290384, + -42.45226502704055, + -42.18397429266377, + -41.92212531907494, + -41.64374813716246, + -41.35245730080967, + -41.06432149035486, + -40.761467928484095, + -40.454594322204976, + -40.150273450651085, + -39.8354963456903, + -39.51103483054273, + -39.18926132755797, + -38.859115427458796, + -38.52251769893648, + -38.19902540483133, + -37.86218669225267, + -37.50934723842243, + -37.16581799944193, + -36.82223660091939, + -36.47303888228664, + -36.123074121166894, + -35.782204882836574, + -35.442409260937794, + -35.102056320517846, + -34.78629987780564, + -34.471883439756084, + -34.14746347121026, + -33.84605388489217, + -33.553708101905876, + -33.250563454522336, + -32.96460424785478, + -32.68641759692767, + -32.40952811630079, + -32.13647907841946, + -31.875496718347694, + -31.62040906978592, + -31.3692731568516, + -31.128653885588243, + -30.895484754384583, + -30.673021061602277, + -30.4564499076067, + -30.243102858337505, + -30.037110492090004, + -29.840072342414665, + -29.646596192747868, + -29.45520323757677, + -29.27322605585274, + -29.092874082076392, + -28.911547352606625, + -28.736870940234013, + -28.569567648391725, + -28.404831342714697, + -28.246515067547016, + -28.097330488214077, + -27.946981831102516, + -27.797469042688693, + -27.65824620050871, + -27.518667348820248, + -27.377270208336014, + -27.243466225191476, + -27.108572634055996, + -26.970365830634698, + -26.836377997977227, + -26.70039720608668, + -26.56266711343642, + -26.426854755029865, + -26.290415675286066, + -26.149065128213227, + -26.008000462419282, + -25.868400980166317, + -25.72419946234665, + -25.583747645422825, + -25.44457752090553, + -25.304002733636057, + -25.16402169134971, + -25.024850126056847, + -24.886710853992444, + -24.75077367734998, + -24.612215773194514, + -24.475219884022657, + -24.33902673184721, + -24.20212450543729, + -24.068034805090296, + -23.934360297636253, + -23.80302587282976, + -23.671934351942323, + -23.541636228121696, + -23.41150044334196, + -23.28447644210979, + -23.148561389650993, + -23.025401165750644, + -22.90954852775124, + -22.796729788349747, + -22.684810104200672, + -22.57605822622979, + -22.46792378791558, + -22.361120401782223, + -22.261551301205262, + -22.16353455580876, + -22.064981855155263, + -21.966975526131932, + -21.87150168248146, + -21.772723029819876, + -21.66942948268381, + -21.57520608187572, + -21.484674383312882, + -21.39262706670191, + -21.30338594612345, + -21.220403163730513, + -21.136668215983228, + -21.050642316748583, + -20.971266422092764, + -20.893704719430318, + -20.819095103855986, + -20.744356394558064, + -20.668110426183997, + -20.593575790907362, + -20.51913769651664, + -20.44398529835246, + -20.37019633369734, + -20.298325499929867, + -20.222271214700026, + -20.14286692746316, + -20.068865753641326, + -19.994722344735, + -19.921408639803634, + -19.848279216226192, + -19.77733712660915, + -19.708843692405654, + -19.639876490727172, + -19.571363877498232, + -19.500457183976046, + -19.42755711057401, + -19.353821194607914, + -19.277222947707447, + -19.198696655644017, + -19.117528511903302, + -19.032573119760347, + -18.948416948005327, + -18.8617110322628, + -18.76787339255267, + -18.671976260282626, + -18.572269788069963, + -18.465027304774082, + -18.35302653550974, + -18.23322606697562, + -18.109198924002346, + -17.97901877700008, + -17.8424357393355, + -17.699650572565574, + -17.546991433374593, + -17.388572184205284, + -17.22651462273087, + -17.05426537516221, + -16.875136298756033, + -16.696253347316734, + -16.505823524083787, + -16.312044657667926, + -16.121715044231273, + -15.918228961012252, + -15.713662817862469, + -15.520349065238337, + -15.314117388062634, + -15.101981873349454, + -14.901015296769765, + -14.692722931406239, + -14.47507862921057, + -14.264177112491339, + -14.051834892143809, + -13.828488818514472, + -13.610171729805234, + -13.392998744052473, + -13.14431326671235, + -12.919627335562577, + -12.694532034725631, + -12.46382295349791, + -12.231529775858943, + -12.002729134096949, + -11.770840032936897, + -11.545372404882938, + -11.319961015720121, + -11.094155532004333, + -10.870343496667056, + -10.649476851254931, + -10.432298095911957, + -10.216307836805138, + -10.001517123035553, + -9.790703986991717, + -9.580788044014426, + -9.370481683762973, + -9.16321502127577, + -8.961535107073983, + -8.759009779363042, + -8.557318526251073, + -8.355886338897776, + -8.153905396308813, + -7.951682623975034, + -7.75027452716869, + -7.5512766894352366, + -7.350702686380873, + -7.154769967425929, + -6.960011268123255, + -6.766274518296466, + -6.572764187927147, + -6.37801869391982, + -6.186990444418874, + -5.9952547142450285, + -5.8024834919889425, + -5.610113250581571, + -5.421421402540372, + -5.230705688924124, + -5.046683022305792, + -4.872716593779623, + -4.6951301980288855, + -4.511179217587073, + -4.332158443315197, + -4.158427781620778, + -3.9781560199225248, + -3.804624532867186, + -3.6385278102201566, + -3.4657404385840342, + -3.2985543996196376, + -3.143760131127154, + -2.990897550279068, + -2.853284988059845, + -2.728124024093954, + -2.6083346915128622, + -2.4995781005502993, + -2.401952119700064, + -2.3136789494691223, + -2.238971619951746, + -2.178121549782583, + -2.1308086498294294, + -2.0989279496834654, + -2.0811384019076833, + -2.0791462567281584, + -2.0941115217407553, + -2.127357150261988, + -2.1812564930478997, + -2.255843138179999, + -2.351285321542093, + -2.470949885461389, + -2.619418061092849, + -2.7877105289187534, + -2.975052188900128, + -3.186661968551388, + -3.4111859653665855, + -3.653318850390739, + -3.9126615935363556, + -4.1800908551126845, + -4.467339948394169, + -4.76213772483465, + -5.057079131423051, + -5.366431454037695, + -5.686629453715802, + -6.000219054441521, + -6.315818757227649, + -6.636045568944125, + -6.948817367054708, + -7.261130271977808, + -7.577241231467479, + -7.888166201756551, + -8.197871795054352, + -8.50808823068634, + -8.812478521149504, + -9.115105767042023, + -9.414291744215312, + -9.711444317474582, + -10.007502270019703, + -10.299487170473892, + -10.592538344186792, + -10.891153498312034, + -11.187520981503875, + -11.482022983094124, + -11.780385466250532, + -12.081554951147046, + -12.381652289882577, + -12.682140856885521, + -12.990862659199513, + -13.299742955942214, + -13.611617440190054, + -13.934305403208066, + -14.256927111462618, + -14.586113170885733, + -14.917746982119734, + -15.251391235544043, + -15.594129593187766, + -15.940412518175888, + -16.28431813443251, + -16.642108913478406, + -16.995968507434565, + -17.341447380945205, + -17.68624308935871, + -18.046967535864507, + -18.40739677468867, + -18.75423808809474, + -19.122883537464627, + -19.494562086443743, + -19.850467709603112, + -20.217375693444236, + -20.600471912205226, + -20.964774255638137, + -21.334574125398436, + -21.71635453927342, + -22.08321685900946, + -22.451794632413858, + -22.830192417191082, + -23.197398613331025, + -23.56588574348082, + -23.9411828469809, + -24.317713222784892, + -24.691862653126957, + -25.067342381426663, + -25.446518281020253, + -25.823575223588996, + -26.2028494895185, + -26.583118605135837, + -26.966954059517665, + -27.35077087822334, + -27.734676910582905, + -28.118658584974153, + -28.49849501805635, + -28.87466346669121, + -29.25502327108787, + -29.62282677381422, + -29.98810194848637, + -30.36409863788679, + -30.729381056802502, + -31.093416129346256, + -31.462290431553313, + -31.82712487808434, + -32.183114856408764, + -32.54546634304612, + -32.91461519520696, + -33.27211085075718, + -33.634882201375724, + -34.00379296069003, + -34.365538004602506, + -34.729305012827695, + -35.09053582815073, + -35.442051401313115, + -35.80147397332604, + -36.156478428285936, + -36.509943348927166, + -36.87540585234159, + -37.23901072209325, + -37.60450716369108, + -37.98091776311527, + -38.35826093083761, + -38.72586756756725, + -39.099598938468134, + -39.47442917873935, + -39.83691641541903, + -40.204012329161955, + -40.574518240633275, + -40.93284919279329, + -41.286376866942405, + -41.64670181348839, + -42.002212553600565, + -42.3507670717366, + -42.710882038521, + -43.06959132588439, + -43.421435885960044, + -43.7807719502002, + -44.13042410569125, + -44.47974523087207, + -44.83083555170508, + -45.17975029971333, + -45.526137563452274, + -45.875589050399924, + -46.22206441573103, + -46.569064381392046, + -46.91316488006124, + -47.252871500956985, + -47.59460254898223, + -47.93472843239765, + -48.27245989973975, + -48.61316785031234, + -48.95780558244229, + -49.30319506797552, + -49.64442423993319, + -49.98381841805986, + -50.32604717784858, + -50.66631640577856, + -50.99451219306471, + -51.32925243718483, + -51.66956924331703, + -51.998736515246826, + -52.3287767677094, + -52.667858253196044, + -53.01000201520464, + -53.36753357541763, + -53.73099880724471, + -54.095482201916454, + -54.451681350020955, + -54.80973824207325, + -55.17013669730103, + -55.524106384949896, + -55.8756427780986, + -56.22684818609178, + -56.568173790368725, + -56.89164961503797, + -57.210054830820965, + -57.527142887254534, + -57.84681566068695, + -58.1570298021242, + -58.463839291949, + -58.77464051965263, + -59.080840617557335, + -59.379611364919995, + -59.676143211914265, + -59.95963755435715, + -60.239955016541984, + -60.52007216423825, + -60.80366715994708, + -61.08194029433051, + -61.35281209135889, + -61.61752644560808, + -61.88090988763418, + -62.17169681900389, + -62.41810160592986, + -62.67171699605905, + -62.92700286480446, + -63.171093429782275, + -63.40996922133053, + -63.648671884466815, + -63.881023424434204, + -64.11161156677625, + -64.33274081599845, + -64.5484139784681, + -64.76538019545904, + -64.9762134188021, + -65.20740470348781, + -65.40373475442904, + -65.59756447844596, + -65.80873944884215, + -65.99163835726283, + -66.1642621214959, + -66.32658432151234, + -66.48021806007075, + -66.62097019240412, + -66.74939135202578, + -66.87171558534783, + -66.97876032382108, + -67.07211215897057, + -67.15447761613395, + -67.22104224477765, + -67.2741529478326, + -67.31308135995369, + -67.33853409042516, + -67.35370455384614, + -67.35460539923463, + -67.33965814033566, + -67.30902899364848, + -67.26404469862514, + -67.2078001782688, + -67.13824374251357, + -67.05358555831275, + -66.95710528627973, + -66.84693482330073, + -66.72309386440044, + -66.58992031612112, + -66.44468496860021, + -66.29607605628406, + -66.14247628542849, + -65.98439382705837, + -65.82077959233854, + -65.65819730422088, + -65.49709534143815, + -65.32840960359943, + -65.16012022721834, + -64.99283673717963, + -64.81902387014065, + -64.64236745794976, + -64.46675564704715, + -64.28902454181636, + -64.10627233332251, + -63.9245189968719, + -63.739895374679506, + -63.552789948909144, + -63.36255950167904, + -63.171640173338844, + -62.977289859578626, + -62.78089205260735, + -62.58559801107137, + -62.385988271432005, + -62.18664524096537, + -61.98937501139463, + -61.78934089875508, + -61.585390276013854, + -61.38256301127122, + -61.174410387424665, + -60.961679105118876, + -60.74845623489055, + -60.533814350995335, + -60.31690206841151, + -60.10096530121553, + -59.882987962738184, + -59.66207909539635, + -59.44231655656435, + -59.21722167022417, + -58.98802951173427, + -58.76205771207255, + -58.53723530877152, + -58.30593089017669, + -58.08199102346705, + -57.86537305766054, + -57.63728994130255, + -57.417093154127556, + -57.19783982644442, + -56.96809559375092, + -56.745401588101466, + -56.527400212898556, + -56.302759666354554, + -56.08121448281064, + -55.86963629564599, + -55.65688287615789, + -55.43401671720818, + -55.21534072806675, + -54.99965622634113, + -54.77522794277379, + -54.550565532425885, + -54.33252560499146, + -54.10737652938803, + -53.87845931182021, + -53.65398134783925, + -53.423887931321886, + -53.18849279457918, + -52.96138067614139, + -52.73633495991684, + -52.511767140085105, + -52.293945992345314, + -52.07486610261543, + -51.86308025501084, + -51.64710141416969, + -51.4285046739303, + -51.207936041427956, + -50.98112666724028, + -50.75759524732726, + -50.530524647357275, + -50.29611717891968, + -50.05853979565974, + -49.82148790953997, + -49.579964240667685, + -49.337494641666325, + -49.099491975439534, + -48.86009979050132, + -48.63134517445355, + -48.40446603377749, + -48.15076643023613, + -47.91352750034972, + -47.68602777604388, + -47.4606470698996, + -47.22363976674354, + -46.98960292127097, + -46.75719495268253, + -46.526941675163066, + -46.28666461660954, + -46.049829381604496, + -45.818999186174935, + -45.590109752221245, + -45.34974987102058, + -45.11066667581404, + -44.882333624532365, + -44.64177533187349, + -44.38690967518651, + -44.13745327703394, + -43.89167800014418, + -43.6346765855797, + -43.37106746872596, + -43.120170567487335, + -42.87514780108498, + -42.61189937175488, + -42.3444472783157, + -42.08199659401624, + -41.80345028441472, + -41.514048526503196, + -41.22651686968583, + -40.92555981169007, + -40.620679684726795, + -40.31489772743544, + -39.99940914928018, + -39.672597049306184, + -39.34823267667472, + -39.01389573865975, + -38.67111447699712, + -38.3417180772891, + -37.99869991142855, + -37.64066093269598, + -37.29183094018727, + -36.944293079592875, + -36.59102497377393, + -36.240332036105386, + -35.903320575532284, + -35.56515687007894, + -35.23029923900151, + -34.92269420854079, + -34.614763213636685, + -34.29892708616655, + -34.01210940746176, + -33.72535284716105, + -33.43346597443473, + -33.16069412386568, + -32.893568944678464, + -32.618449071051465, + -32.351954603625344, + -32.09740388948943, + -31.843371052113444, + -31.591763948974286, + -31.34816043213504, + -31.111338409549965, + -30.884495494391107, + -30.660592507642257, + -30.43986832898685, + -30.232311921187634, + -30.034832536263558, + -29.836882422200684, + -29.647805933322292, + -29.464161681522267, + -29.278335121264682, + -29.09703501070668, + -28.92426903504328, + -28.752792917301697, + -28.57969357253216, + -28.419568014755612, + -28.264885833516104, + -28.105049263851885, + -27.953223004070953, + -27.809238189289864, + -27.658637193310675, + -27.51092928949594, + -27.368130468550394, + -27.22228630232354, + -27.078306792113878, + -26.93666850605959, + -26.793280175586176, + -26.647893986261852, + -26.506868769852076, + -26.362206492738075, + -26.213789442975205, + -26.065900168254448, + -25.917723340433536, + -25.76644333607956, + -25.617643956734327, + -25.469910724251086, + -25.32087796709313, + -25.175205130333048, + -25.02923169482956, + -24.88858201198964, + -24.744508251223348, + -24.599890378823794, + -24.45614000552284, + -24.31174841617279, + -24.169613655949668, + -24.025554899223497, + -23.88392619934849, + -23.744495502265423, + -23.603328354286806, + -23.46321450382037, + -23.326411948354405, + -23.195045748240727, + -23.06177641883814, + -22.93121302256855, + -22.80615728391779, + -22.68289856818865, + -22.56415621191781, + -22.447976935762057, + -22.333911497446024, + -22.22793228306158, + -22.124055858807413, + -22.01852245969138, + -21.91624251358265, + -21.819172107053053, + -21.71383450795761, + -21.60779321331262, + -21.51057160389064, + -21.413535363175278, + -21.313846004326333, + -21.221748340988928, + -21.132012972374937, + -21.034140729436047, + -20.940020427879332, + -20.84880348241341, + -20.757562254644483, + -20.666031727970818, + -20.574703883397184, + -20.482314164119668, + -20.38841150430447, + -20.29335029544132, + -20.200865709932508, + -20.11118814062886, + -20.017291769726018, + -19.925399536618944, + -19.836184497930848, + -19.74708590869075, + -19.655944044723206, + -19.569553194373647, + -19.48507098397482, + -19.398711486551836, + -19.311303325113908, + -19.218866146453074, + -19.122777395645734, + -19.023391537244862, + -18.921415244909824, + -18.815847394177677, + -18.706022277471703, + -18.59663693758805, + -18.481433923823037, + -18.360150135271645, + -18.23514641089734, + -18.11650148987277, + -17.998273267605136, + -17.873623826276784, + -17.744163833825844, + -17.611702852220468, + -17.475956962954065, + -17.333844386239388, + -17.185815383871923, + -17.036862941336874, + -16.88543948550195, + -16.71508023090852, + -16.539657270708158, + -16.361528991185704, + -16.175309519380576, + -15.992975878334086, + -15.809859644432564, + -15.613581896340465, + -15.425261356390541, + -15.24047317195969, + -15.035840578274309, + -14.84650269721174, + -14.670204811131125, + -14.475004061424876, + -14.277627277337423, + -14.085006727846713, + -13.883696292373584, + -13.6760943845444, + -13.475113325042148, + -13.273863511387828, + -13.06761417066516, + -12.85571265687709, + -12.642648543091907, + -12.426046847370069, + -12.206646246010985, + -11.989051906594568, + -11.767208718892018, + -11.548567712995542, + -11.327059003885726, + -11.108289901515315, + -10.89483712861106, + -10.651596952608092, + -10.402437268413932, + -10.154654641580422, + -9.903982397230568, + -9.655577505284743, + -9.410451765981142, + -9.167145777093761, + -8.922410420456462, + -8.677950678765317, + -8.442878178667304, + -8.224549091238233, + -8.012244108645312, + -7.803377836943842, + -7.5992916658262235, + -7.3905951917552155, + -7.180661574635478, + -6.978917859540822, + -6.775344940328747, + -6.5707139938575985, + -6.369800614404133, + -6.16294179079084, + -5.955294821820569, + -5.7494593187793015, + -5.53920160205076, + -5.327561320334729, + -5.127665088135819, + -4.923495928887167, + -4.716923102821366, + -4.522760306960668, + -4.324679740492083, + -4.1349298904023835, + -3.9574413344576356, + -3.779892597270801, + -3.603220899922884, + -3.435553303622606, + -3.272291094171636, + -3.1088952667081533, + -2.9554104392099503, + -2.8056806730677, + -2.661758505747535, + -2.528416841861101, + -2.397186683423948, + -2.27517888973646, + -2.164584053072101, + -2.0615658461716873, + -1.9662971884350573, + -1.880455250454103, + -1.8062125159846574, + -1.7416060408752518, + -1.6883575246470086, + -1.6464221118954492, + -1.6161019349254333, + -1.598952334573434, + -1.5962239149252784, + -1.6088778091433145, + -1.6365442606316238, + -1.6817466445111175, + -1.7471709239700712, + -1.8304379054912938, + -1.9293634624295373, + -2.0557765917490776, + -2.2106226245532574, + -2.379448232045034, + -2.5642431789587876, + -2.770485432445319, + -2.984658334705341, + -3.2125196137460006, + -3.4560509928231054, + -3.7074353443265613, + -3.9715840351858427, + -4.261318533939599, + -4.524814887744411, + -4.801974158007338, + -5.086049453114505, + -5.3717500378122764, + -5.661918145237242, + -5.951433251046758, + -6.239212786650746, + -6.528322701353272, + -6.816435192112626, + -7.093688572207026, + -7.380497893714439, + -7.669322202948817, + -7.957540775263811, + -8.240558247756539, + -8.526147085033289, + -8.8094926928902, + -9.088670509354857, + -9.370651659488038, + -9.64811978897924, + -9.92364255285066, + -10.204372674559558, + -10.482359883396267, + -10.759157124840334, + -11.041214801482244, + -11.323874997705309, + -11.607346828275382, + -11.892572715090681, + -12.179160979136796, + -12.47143522716537, + -12.764556620481914, + -13.057292124639014, + -13.353928227169805, + -13.654242722158928, + -13.957140782146217, + -14.264127794442901, + -14.573264478042214, + -14.887181306730454, + -15.20907860518733, + -15.526499329387255, + -15.849211824472095, + -16.1837218211788, + -16.51242832777449, + -16.844908402576852, + -17.17604128776223, + -17.52185730380651, + -17.86363848644997, + -18.195146483620313, + -18.546750146028806, + -18.89652427529972, + -19.233422785718236, + -19.58001510150497, + -19.93962503028559, + -20.288186067872886, + -20.63609329753014, + -20.995772633526173, + -21.348988875525382, + -21.69659190603601, + -22.05360912650614, + -22.407707415262625, + -22.75424121571816, + -23.108273307268206, + -23.456800500960327, + -23.81209383041351, + -24.16824531704238, + -24.52007929669434, + -24.875864462154343, + -25.234450167280023, + -25.59249566257504, + -25.94840914936964, + -26.312619561636854, + -26.67225056181053, + -27.03042670043228, + -27.393002228059295, + -27.750145402605973, + -28.056119222177905, + -28.406556474352666, + -28.755716255364483, + -29.09454159159732, + -29.43946894773987, + -29.7860694420215, + -30.12272349580964, + -30.47029680231665, + -30.813465404750218, + -31.14857999508093, + -31.489246573379937, + -31.83343464508342, + -32.180106428740125, + -32.5181274336786, + -32.859070374984235, + -33.2098595497299, + -33.553603501315536, + -33.89825888738154, + -34.237887134155855, + -34.56665357821295, + -34.899538291821145, + -35.22762919113327, + -35.55176383432408, + -35.87623956047461, + -36.201292388560894, + -36.5207841866333, + -36.84531761093447, + -37.165586545365954, + -37.475087034752455, + -37.781226759500434, + -38.07977000686565, + -38.36833329366603, + -38.64527235782367, + -38.95212376368915, + -39.22191254840864, + -39.476727235435135, + -39.72943703874776, + -39.978926081087, + -40.21587167988429, + -40.44927422439422, + -40.67779494125739, + -40.89732908766, + -41.10698439454845, + -41.31169356236648, + -41.50934421279334, + -41.70070147597102, + -41.885089193801406, + -42.0623696183549, + -42.23242790307994, + -42.3975268493152, + -42.554139770735866, + -42.706005443434805, + -42.85119877794946, + -42.99326441183154, + -43.13043244029614, + -43.26536152526621, + -43.396386244919455, + -43.525692046003925, + -43.645579472730375, + -43.76058152027081, + -43.87314104004291, + -43.97967154488688, + -44.08086289472793, + -44.14137311119168, + -44.23687926633128, + -44.32673352167267, + -44.411268496572475, + -44.488690502888126, + -44.563273244780454, + -44.63163749042488, + -44.69691641427846, + -44.76166908041577, + -44.82416156640577, + -44.88329338084764, + -44.938867626121535, + -44.991684912561816, + -45.04115679053738, + -45.084374913991404, + -45.12328908023327, + -45.158615138261865, + -45.18908164182476, + -45.21715773717338, + -45.241843955700894, + -45.26225978074167, + -45.2812397669999, + -45.29912954405049, + -45.31495342648793, + -45.32909914608487, + -45.34129563765416, + -45.35123159378307, + -45.36002550833295, + -45.367429663092, + -45.373450790889294, + -45.37892491568039, + -45.38348451296948, + -45.38709576122565, + -45.38987259486149, + -45.39201223749755, + -45.39378837231216, + -45.39522130842233, + -45.39638158814507, + -45.39745662734854, + -45.39852915472637, + -45.39959623986342, + -45.40060689908244, + -45.4009172382536, + -45.40117706279354, + -45.40140460923213, + -45.40163051313254, + -45.4018254182305, + -45.40199439764787, + -45.40215732635067, + -45.40230463500697, + -45.402431118078326, + -45.40253451824544, + -45.40347533642653, + -45.4045009196911, + -45.40552077161054, + -45.406543967452464, + -45.40756378700323, + -45.40413971615596, + -45.405139804092926, + -45.40612640666854, + -45.4071109824521, + -45.40809951205476, + -45.40833313628159, + -45.40847880006112, + -45.40862585447327, + -45.40876694322939, + -45.40890264220816, + -45.40903780334378, + -45.409165907549266, + -45.40928989672995, + -45.40941046317816, + -45.40952603744997, + -45.409900894210814, + -45.41028605882007, + -45.41065411389173, + -45.41100188576985, + -45.41133135166681, + -45.4116413057063, + -45.411932882404905, + -45.41220387265481, + -45.41245398582146, + -45.41268703353797, + -45.412762329630134, + -45.41281787658448, + -45.412867679042265, + -45.41291217928194, + -45.41295127054923, + -45.41298450778652, + -45.41301281485027, + -45.4130352475178, + -45.41305221925071, + -45.41306374612866, + -45.41309799695799, + -45.413099614007116, + -45.41306145129848, + -45.41298698123988, + -45.41287818974583, + -45.41273854380772, + -45.4125637350658, + -45.41235486001969, + -45.412116954526766, + -45.411841286248574, + -45.41177303123894, + -45.411730072958946, + -45.4116850162911, + -45.411637649842746, + -45.41158623264187, + -45.4115310064707, + -45.41147140268202, + -45.411400723873406, + -45.41133185972873, + -45.41126062736721, + -45.41124040327362, + -45.41122029569904, + -45.411202513724646, + -45.41118365673517, + -45.41116019455542, + -45.411136594624296, + -45.411114116089095, + -45.41108971761326 + ], + "xaxis": "x", + "y": [ + 45.30065635922624, + 45.30124205437229, + 45.30193400755794, + 45.30265046529222, + 45.303148779253284, + 45.30355241309474, + 45.30436763201242, + 45.30509261653337, + 45.30601404203569, + 45.306807104326325, + 45.30755378323268, + 45.3084595882965, + 45.30939829386062, + 45.310166602457215, + 45.31092652284556, + 45.31184263146779, + 45.31269093838768, + 45.3135576272191, + 45.31414556023766, + 45.31474025871411, + 45.315360652914144, + 45.31605557841787, + 45.31664194012994, + 45.31730697535071, + 45.317890613009425, + 45.31851538132313, + 45.319104667384806, + 45.319609469309746, + 45.32007727141195, + 45.320588366136754, + 45.32098028619176, + 45.32137490542775, + 45.3218027568607, + 45.322205865337594, + 45.322700240070716, + 45.323155095223285, + 45.32354950746685, + 45.32388409129931, + 45.32428643617517, + 45.324720620555965, + 45.325017845529025, + 45.32541910711946, + 45.32594672200731, + 45.32622930094886, + 45.32648902428704, + 45.32678770395347, + 45.3271690999394, + 45.327448012220245, + 45.32774646010955, + 45.32811341332325, + 45.32845006239287, + 45.328854892378736, + 45.32925256389109, + 45.32956418507035, + 45.3298872080439, + 45.330206948208186, + 45.33058368137483, + 45.330898532790556, + 45.33111427854165, + 45.33145221325836, + 45.331679937477944, + 45.331990641078995, + 45.33232540056891, + 45.33257673594378, + 45.33284641602727, + 45.33315718987583, + 45.33344664049978, + 45.3341012148372, + 45.33498707332209, + 45.33579373852934, + 45.33656718067833, + 45.337500908131304, + 45.33850912961951, + 45.339404503757684, + 45.34025240597607, + 45.34119504699709, + 45.34225198723166, + 45.342975385452, + 45.34373765087828, + 45.34457141712823, + 45.34520671973865, + 45.3459652914809, + 45.3468944913872, + 45.34763544710913, + 45.34834020280647, + 45.34927992179219, + 45.350152851503346, + 45.35111122525774, + 45.35222320834729, + 45.353302302900545, + 45.35436280634842, + 45.355351858982594, + 45.35636600921168, + 45.357455718978656, + 45.35846858023884, + 45.359484076225094, + 45.36044248198346, + 45.36136159825707, + 45.36201903786664, + 45.362607731819615, + 45.36338685277333, + 45.3641318443685, + 45.36464971564687, + 45.365223329150176, + 45.365707271298, + 45.36644051487314, + 45.36727776004473, + 45.36789140847082, + 45.36862781694657, + 45.36935732697808, + 45.37007171229736, + 45.37093491702046, + 45.37173675951087, + 45.37239239138229, + 45.37311424419294, + 45.373893642577876, + 45.3747371400346, + 45.37557952956787, + 45.37617997045989, + 45.37676707820621, + 45.37752752882974, + 45.378324959843155, + 45.379006673860545, + 45.379605142930025, + 45.38034676039454, + 45.381162078651684, + 45.38194037878425, + 45.382527586665006, + 45.38295425868896, + 45.383675463602145, + 45.384443316479214, + 45.38494781921849, + 45.385568689166135, + 45.38640871629172, + 45.38693699116032, + 45.38780653396654, + 45.38835553836785, + 45.38885891735504, + 45.389154530847094, + 45.3898332667208, + 45.39055161538907, + 45.39088370020398, + 45.39142454130535, + 45.39206677437021, + 45.39272727527996, + 45.39351633320761, + 45.39419795561232, + 45.39471870690721, + 45.395467328939716, + 45.3965562787684, + 45.397069915703106, + 45.397574952630535, + 45.398274735105915, + 45.39902863728136, + 45.39967158075588, + 45.40013911515351, + 45.40050842848147, + 45.400799460712484, + 45.40146284138777, + 45.402032107932655, + 45.402394761819686, + 45.40278992019668, + 45.403446170751124, + 45.4042712768712, + 45.40541550236088, + 45.40594147911467, + 45.40645103491125, + 45.40736818440101, + 45.40877140195812, + 45.41076173057873, + 45.41213695009949, + 45.413260271751, + 45.41488591444327, + 45.41659224102789, + 45.41814194246695, + 45.41967431722273, + 45.42122421702902, + 45.42268026571465, + 45.42412747733741, + 45.4256593487682, + 45.427073589205904, + 45.42846906335461, + 45.42995491671941, + 45.431508868967576, + 45.43294949070756, + 45.43449795571096, + 45.436045896077744, + 45.43728591240688, + 45.43830163804861, + 45.439456093556636, + 45.44079735860992, + 45.44215224015999, + 45.44343120932357, + 45.444375003035816, + 45.445473560628116, + 45.44677225297314, + 45.44797609993086, + 45.44904543456109, + 45.44984120341096, + 45.45083794348416, + 45.452172561035646, + 45.45301431712992, + 45.45396179300497, + 45.45509309570998, + 45.45656097111738, + 45.458946085778955, + 45.460441648941014, + 45.462685237721104, + 45.46475459325952, + 45.466758441210935, + 45.472838358761976, + 45.484788098251514, + 45.50565315221926, + 45.53818408883597, + 45.5837903752552, + 45.64111097475223, + 45.704187093594705, + 45.76522474138874, + 45.815936438604616, + 45.856251405721544, + 45.88558165611295, + 45.907505296726434, + 45.923163721268345, + 45.93400337920274, + 45.94351507725153, + 45.955115010582595, + 45.972050141859455, + 45.999918103421045, + 46.03983971828734, + 46.09248763925718, + 46.158414662017144, + 46.23703736619839, + 46.327072657177446, + 46.42859592850699, + 46.541639647834934, + 46.667231187151486, + 46.8061285382561, + 46.957309432679715, + 47.118302808644636, + 47.28683927792054, + 47.463314218233265, + 47.64834611054478, + 47.83566798625701, + 48.023172248939865, + 48.20956769266305, + 48.39662784137522, + 48.57955770996967, + 48.75555971609832, + 48.92948050858245, + 49.093625856117846, + 49.24649494120659, + 49.393815701230196, + 49.52991360445883, + 49.65438721604678, + 49.76523014549145, + 49.8657542055838, + 49.95354106003255, + 50.02452950595315, + 50.081488363030104, + 50.125347509952476, + 50.153806861592756, + 50.16461030378974, + 50.15624106610237, + 50.12911427774848, + 50.083836488523914, + 50.02017330276769, + 49.93702726391105, + 49.836609476753964, + 49.723397604679526, + 49.58920810115112, + 49.43606536365915, + 49.28353242203603, + 49.10416673639415, + 48.90626942969818, + 48.70003420605666, + 48.478655587866776, + 48.25004021935163, + 48.00576350908627, + 47.74332031709656, + 47.48982967303377, + 47.22313318201167, + 46.93662462568459, + 46.659443183046186, + 46.37493857465736, + 46.07180596975934, + 45.76914529901862, + 45.464300795957406, + 45.15069647264574, + 44.82778404522427, + 44.504214552495256, + 44.175672589939694, + 43.84039272948602, + 43.50903893858218, + 43.174720354105986, + 42.826041969874936, + 42.48349316746876, + 42.139016908716485, + 41.786461125674016, + 41.41556209947847, + 41.064403391404404, + 40.70602954504817, + 40.32807491275439, + 39.959394441926364, + 39.59687493350447, + 39.2248773385048, + 38.84348071036869, + 38.48260607577791, + 38.11372693814414, + 37.73017677472638, + 37.359222962492275, + 36.99840874913466, + 36.624498682198684, + 36.256436845624044, + 35.89994905495483, + 35.532620226739965, + 35.167989300368006, + 34.80099922747861, + 34.44037918294277, + 34.07418353586902, + 33.70399761498599, + 33.34426143382196, + 32.98334220062625, + 32.62887100293793, + 32.2600001708878, + 31.91429267155726, + 31.567862840036128, + 31.20477116469836, + 30.852135211188124, + 30.5041019515858, + 30.15437616855528, + 29.795493418235086, + 29.439773034543396, + 29.092867442925655, + 28.74074003409194, + 28.389689200180523, + 28.04108618783154, + 27.694376883639453, + 27.343390182357584, + 26.99807895619347, + 26.654587155780035, + 26.298581014537678, + 25.96005462606094, + 25.62522181335514, + 25.276650295123694, + 24.933256141845575, + 24.600249322179046, + 24.25324649044085, + 23.912317605463883, + 23.578091177360236, + 23.238691252751593, + 22.913128686706337, + 22.587558377602974, + 22.260670840822804, + 21.945386829709058, + 21.632131528260782, + 21.309847674618073, + 21.00083876508177, + 20.692869915956067, + 20.374059350389338, + 20.069267127553385, + 19.76556702024636, + 19.45555588051149, + 19.152908458260175, + 18.853670810727607, + 18.54332865279199, + 18.233151029185514, + 17.931059701218214, + 17.621971149590077, + 17.314731946458686, + 17.01364025257034, + 16.714380610703095, + 16.411528787587354, + 16.109089073258097, + 15.805912897581694, + 15.502517862735598, + 15.194229158304234, + 14.887743611100595, + 14.580535534255668, + 14.267081270570092, + 13.957935779247167, + 13.645837748807345, + 13.329297699110638, + 13.017084780374136, + 12.708165383851261, + 12.391605224475741, + 12.076588495210615, + 11.771657252356125, + 11.428856918233718, + 11.115085510666857, + 10.811547455719008, + 10.508092171074775, + 10.202100623559728, + 9.902107827661291, + 9.606825690276896, + 9.310224501023288, + 9.016231326587475, + 8.724466119535888, + 8.429646961681588, + 8.141380509861708, + 7.847680658910126, + 7.548387398439552, + 7.2557844820569155, + 6.958131601985776, + 6.648654056656934, + 6.342844172003127, + 6.0429672685303055, + 5.724886961082017, + 5.414892843547536, + 5.1155752002257335, + 4.793823666094012, + 4.475778975369742, + 4.167811483842318, + 3.8436931817113065, + 3.5206956344703415, + 3.205702010388127, + 2.88152243930825, + 2.570958053977248, + 2.260990016202482, + 1.9440542354900103, + 1.6457803016981958, + 1.344874972001644, + 1.0359357387089771, + 0.7431647760945245, + 0.4470713207390033, + 0.14182208713334643, + -0.14927943330410953, + -0.44086000184198426, + -0.7319241037042714, + -1.01638476911375, + -1.293997171603349, + -1.5693275790496173, + -1.815731952007402, + -2.0444292003357356, + -2.274251979854682, + -2.481389733636101, + -2.6646039940176514, + -2.838972470329237, + -2.9884123218117042, + -3.11023864851491, + -3.21472992012814, + -3.2974032758040073, + -3.355915431645531, + -3.3913835382922035, + -3.4048123854802816, + -3.39803858859178, + -3.3710280857549937, + -3.325108369200027, + -3.2611074439519006, + -3.1802804961620454, + -3.084287327127412, + -2.974114920253723, + -2.8483406607594324, + -2.7114471000777796, + -2.5612456990820673, + -2.3972975979451885, + -2.2240683506681793, + -2.0399820767784145, + -1.8454583819492636, + -1.6462925311363519, + -1.4426765317417052, + -1.2355122652243018, + -1.0266833604456562, + -0.8203287147004672, + -0.6151670102511687, + -0.41186524360498994, + -0.21142850065476018, + -0.014788827337959793, + 0.17912239878214273, + 0.3695248181338274, + 0.5497528631733597, + 0.7202484735149064, + 0.8870487712695617, + 1.044087887633294, + 1.1923079643531156, + 1.3375689256931582, + 1.478910789980186, + 1.6021343359196878, + 1.7198753508456497, + 1.8343477615511954, + 1.935784009373079, + 2.034653048186079, + 2.1293593031519724, + 2.209958860284805, + 2.285664476839112, + 2.3577403536442203, + 2.422468884831358, + 2.4857085427293923, + 2.545101229466147, + 2.5939810802864383, + 2.63730522737304, + 2.6793949775599177, + 2.716254932991565, + 2.750329503332266, + 2.7847351056235126, + 2.8149824253505944, + 2.840858525848436, + 2.866832856374559, + 2.89238014379388, + 2.9184729701399044, + 2.9480580670122456, + 2.979556576918602, + 3.0146754054240814, + 3.054826423352543, + 3.096675035659461, + 3.139990556703625, + 3.184667815612314, + 3.2292327942258465, + 3.2743714193132347, + 3.32226300056172, + 3.371700814595118, + 3.424566973691934, + 3.4830876993069837, + 3.5485035527291973, + 3.619961418131344, + 3.6975441393227633, + 3.781726261926542, + 3.8735972866285193, + 3.9715187448803877, + 4.077521685436444, + 4.190621417091774, + 4.311281690122221, + 4.437588575041153, + 4.569883254801034, + 4.7074473641466374, + 4.8516013531248126, + 5.002459568735386, + 5.157973202353776, + 5.32030188422588, + 5.489787275327103, + 5.664176150901903, + 5.841915989421802, + 6.025743569696181, + 6.215099677021708, + 6.4056394329494495, + 6.599747195124951, + 6.796871578543679, + 7.000876578665662, + 7.210515601989129, + 7.414008863247476, + 7.619599845586016, + 7.833515395655514, + 8.04457660123133, + 8.25985916135352, + 8.481300158080627, + 8.703533225016418, + 8.922489518320095, + 9.142777567531969, + 9.366623691436404, + 9.591344715497799, + 9.819215722954493, + 10.057646146450304, + 10.29427445882235, + 10.526748962602603, + 10.7706706591195, + 11.015257917073688, + 11.253385326295108, + 11.506395584240968, + 11.758353643179158, + 12.005630872525707, + 12.267258033357656, + 12.530909801550804, + 12.786219345300823, + 13.057092854792527, + 13.32675583986419, + 13.587365823600374, + 13.862357481099695, + 14.134623496051223, + 14.394828503880833, + 14.66369232146545, + 14.921908316794884, + 15.18178455458598, + 15.44913444616254, + 15.710475691890215, + 15.966310648688792, + 16.220032134403954, + 16.464998920790475, + 16.71058892492495, + 16.964207762201045, + 17.209136022293695, + 17.461327162857966, + 17.716838938047527, + 17.960616386513582, + 18.210827985277778, + 18.46779453122991, + 18.71105572938212, + 18.97297690737294, + 19.241911854540295, + 19.500014477003784, + 19.767568083880867, + 20.039002069566862, + 20.306012026346778, + 20.564985494761704, + 20.83235000158009, + 21.09764463708101, + 21.35937942682989, + 21.6232834573419, + 21.876158423324846, + 22.128987388840248, + 22.384981603051383, + 22.63557889065227, + 22.886990330602675, + 23.1340420780325, + 23.381549899588997, + 23.63432836002035, + 23.884484257210527, + 24.141551206805165, + 24.410597473604646, + 24.668300827657163, + 24.941057687774595, + 25.235389680352345, + 25.52246640034228, + 25.820733265212404, + 26.146261319287532, + 26.459876076891092, + 26.758775995034405, + 27.083479341448516, + 27.416728140297526, + 27.737446902315366, + 28.074198688632993, + 28.424149427074962, + 28.76398193325348, + 29.105992458154173, + 29.455341264256788, + 29.804127060503248, + 30.15726674063583, + 30.5131652108559, + 30.873861402715676, + 31.24506793191564, + 31.61839907168172, + 31.987082870538966, + 32.36529463382383, + 32.74973439340146, + 33.129260166990576, + 33.507090698883566, + 33.89352452630285, + 34.274803994394475, + 34.62557077132737, + 34.97840526938305, + 35.3422523640149, + 35.67228967036853, + 36.00004151381821, + 36.351047157615085, + 36.67870026120099, + 36.99436114487999, + 37.3308200786596, + 37.65797535807802, + 37.98028943410984, + 38.319243502578786, + 38.65300220649891, + 38.980404389671115, + 39.31352333739406, + 39.6443216231381, + 39.9735661614373, + 40.30608505693576, + 40.64112651950941, + 40.97431678441473, + 41.3094050207714, + 41.639694499983264, + 41.970172151397996, + 42.298154479387556, + 42.63252777091632, + 42.961975135376704, + 43.29487399499332, + 43.63078203673385, + 43.960324113806855, + 44.280587363603054, + 44.61022584117488, + 44.91814656157118, + 45.21551535146278, + 45.52393224649588, + 45.816086558073636, + 46.105626265729946, + 46.40708217324153, + 46.695976393416366, + 46.980681696970024, + 47.302051147569, + 47.59138340375597, + 47.8763230782806, + 48.16904590905884, + 48.46962976416081, + 48.76150376781474, + 49.051398520258836, + 49.347994812593775, + 49.6389114935602, + 49.923916276300005, + 50.21596991028357, + 50.50779320964663, + 50.790358821381034, + 51.07948178153422, + 51.37078619788843, + 51.65129645766725, + 51.93142224600338, + 52.209249332542704, + 52.474303990615155, + 52.74191204645305, + 53.004554558054494, + 53.26258271656714, + 53.52397070358046, + 53.786443744982456, + 54.05305086160046, + 54.316936240388834, + 54.58382060666164, + 54.85971421690486, + 55.139833110999646, + 55.422915124787856, + 55.7068940366129, + 55.9926321361938, + 56.28063917988319, + 56.56640100698341, + 56.845524203622524, + 57.1221086914137, + 57.40251863786673, + 57.68043863229484, + 57.95409464108825, + 58.2316791416228, + 58.51290285072393, + 58.794513578866905, + 59.07724275831911, + 59.358543609297875, + 59.65469311588764, + 59.94482815433133, + 60.22598664665075, + 60.52269524291083, + 60.819740480228326, + 61.09034466522357, + 61.37502407232589, + 61.66738107404477, + 61.92984680065179, + 62.18397791345223, + 62.44359295123307, + 62.680022794538566, + 62.88649249020206, + 63.105048476809586, + 63.317588877884944, + 63.502937534715095, + 63.686564391128805, + 63.86294009574444, + 64.0186306609059, + 64.16673301835688, + 64.30334395151725, + 64.42663562399538, + 64.53651397526073, + 64.63161466900597, + 64.70816300520418, + 64.77281153676934, + 64.82311159398937, + 64.86002769221417, + 64.8892987824187, + 64.9063208822544, + 64.90763321466169, + 64.89550866877087, + 64.8720369381132, + 64.83709216836066, + 64.78896813125499, + 64.72646694884126, + 64.65180600879873, + 64.56676316067332, + 64.46485226581336, + 64.34873269231545, + 64.22535653966474, + 64.0865173517071, + 63.93315970541152, + 63.77116117350705, + 63.59061669908269, + 63.3980218639613, + 63.19137370472949, + 62.96576109058118, + 62.728687979886935, + 62.477231154300924, + 62.21413222611236, + 61.94292872182762, + 61.66377917267032, + 61.377245128534945, + 61.08228224831478, + 60.786882322751936, + 60.48741490343282, + 60.18441305967586, + 59.883376067533106, + 59.58182549475365, + 59.27927534524969, + 58.971913215094816, + 58.656278852646786, + 58.33845528784237, + 58.030759945024286, + 57.71592718520604, + 57.39280002548415, + 57.08655392095955, + 56.77130031276584, + 56.4490628529945, + 56.13098410273622, + 55.822499964849065, + 55.50558258918016, + 55.19053868704912, + 54.88597372525349, + 54.570850756057624, + 54.253975835514126, + 53.94573842453985, + 53.63051043994362, + 53.31091129134796, + 52.993804550829, + 52.67858715138811, + 52.35427618518613, + 52.029122339850936, + 51.709388406735044, + 51.37997236972773, + 51.05126997517338, + 50.72869087137583, + 50.40673533921882, + 50.087407082488724, + 49.76918964500379, + 49.44359393141434, + 49.12774868708828, + 48.80936249396381, + 48.482471009638466, + 48.16154030687558, + 47.83975326351295, + 47.51791137988673, + 47.198017846154094, + 46.87762889692828, + 46.55362901235234, + 46.23150805396997, + 45.91080859581661, + 45.58201851787454, + 45.258302837766585, + 44.93505012540116, + 44.605107474019384, + 44.277477323798685, + 43.955016245727585, + 43.62820678836799, + 43.30942836303323, + 42.98600470233816, + 42.65115161151966, + 42.32784807714479, + 42.001137866568904, + 41.66248222985032, + 41.315973962492706, + 40.9869940930252, + 40.64246110903902, + 40.284759235795846, + 39.93720250367378, + 39.58972856230055, + 39.2391737060455, + 38.881433406578324, + 38.53986266002814, + 38.196382868875865, + 37.844457631283646, + 37.50221303275838, + 37.15913681583543, + 36.81304313884099, + 36.465684266328594, + 36.12490782074833, + 35.78431103636534, + 35.43850879148098, + 35.09626933188624, + 34.74763222137042, + 34.40528131864121, + 34.060431499684526, + 33.709413402553835, + 33.374583192523616, + 33.03659172933373, + 32.69472681307677, + 32.352871024716386, + 32.02382317548636, + 31.692640813105342, + 31.351825875548375, + 30.981931236957166, + 30.649926729282427, + 30.308778463062776, + 29.958820918627342, + 29.612163221650928, + 29.27205607700253, + 28.928528663361543, + 28.587289296537417, + 28.252425888312267, + 27.913438454958953, + 27.575693933844544, + 27.243387472339872, + 26.920450503404965, + 26.593324956162824, + 26.268851354846873, + 25.961662584533766, + 25.648983227043615, + 25.330841804496647, + 25.024094124192995, + 24.716810376168564, + 24.397226452860163, + 24.09106675332628, + 23.792899362983363, + 23.47963434262606, + 23.178586035797643, + 22.89036554768616, + 22.588507931956737, + 22.29472900096752, + 22.016067916560125, + 21.72151298888659, + 21.424651910482837, + 21.14884867245253, + 20.831723694209558, + 20.52882144534976, + 20.251926737154022, + 19.969856166318113, + 19.674857734782908, + 19.394901622127772, + 19.115589157764912, + 18.825826774014903, + 18.540882879612287, + 18.25670131857264, + 17.966870319203498, + 17.679928870075006, + 17.395216912925186, + 17.111623677846932, + 16.83327633895371, + 16.553289938794, + 16.272858561236113, + 15.99475508423096, + 15.714673362984552, + 15.434100350148046, + 15.153104332282986, + 14.878737149287456, + 14.59569361776559, + 14.314400851082752, + 14.037187911041322, + 13.752669178415237, + 13.469130969104476, + 13.186252551954205, + 12.899769263558902, + 12.608741436899221, + 12.320483607054424, + 12.033333952864483, + 11.743212867818992, + 11.452400836491277, + 11.163832061493704, + 10.87428313586358, + 10.581298211797025, + 10.289585233925095, + 10.003460194212307, + 9.714023874585337, + 9.431185913029365, + 9.152679304249583, + 8.876103763020335, + 8.60106861338303, + 8.330234351226155, + 8.062399392062407, + 7.789153086387231, + 7.518843322043353, + 7.244486191065995, + 6.972379311833277, + 6.702715121419891, + 6.397657078386688, + 6.119546934832735, + 5.850730903770537, + 5.5778769180651375, + 5.295731491782003, + 5.010585368516014, + 4.732149618759102, + 4.446223579405364, + 4.154052964445426, + 3.8707771410600462, + 3.578668000483556, + 3.283248346031772, + 3.009831509551147, + 2.7294772185752003, + 2.439359255974464, + 2.165069821066807, + 1.8922510554894079, + 1.611875081193677, + 1.3398181623517504, + 1.06716635467668, + 0.7930661827227454, + 0.5213396865892294, + 0.24900753508883527, + -0.02927115132790787, + -0.30631210960349814, + -0.5903008760638585, + -0.8740797373657807, + -1.1557809442866598, + -1.43457206073578, + -1.7109215467472685, + -1.9875241679544764, + -2.243214593134427, + -2.4798193150215364, + -2.716157479213643, + -2.9366919052011538, + -3.1346932650920354, + -3.323224561917978, + -3.4968741879849956, + -3.6479032785146766, + -3.782963066035635, + -3.8967752603163106, + -3.986063585407675, + -4.05570221046406, + -4.102947137383738, + -4.125668714420832, + -4.126023698323803, + -4.106316079237101, + -4.069879750356919, + -4.013507029812954, + -3.9388505583439106, + -3.8521203595204794, + -3.748246727545021, + -3.628567392124721, + -3.4997206517553705, + -3.356917373552429, + -3.2018029807575537, + -3.037824888284141, + -2.8652333256444793, + -2.684198629368167, + -2.4974028485933752, + -2.306800582453958, + -2.1105985560625293, + -1.9120259660815933, + -1.7137098506786699, + -1.5177394157354076, + -1.3267271944641734, + -1.138814197084552, + -0.9579866704342876, + -0.781498247925053, + -0.6072680113377534, + -0.4379243557395881, + -0.2726371571455989, + -0.11020970900653274, + 0.04568686663126684, + 0.19655028292906002, + 0.33990140254323953, + 0.4746665422254469, + 0.6091185402850376, + 0.7351145318647105, + 0.8559473234029513, + 0.9709084656969998, + 1.0877879580709697, + 1.1983330275124655, + 1.2949339891721148, + 1.3926104580928997, + 1.4861922949559723, + 1.5718667115192695, + 1.6556812775728242, + 1.737533576764661, + 1.8103433203707366, + 1.8768386752259385, + 1.94169337644323, + 2.0008644539328135, + 2.058011016547057, + 2.113376084090168, + 2.161917395027421, + 2.206327915063, + 2.2470241984415624, + 2.2844376943959053, + 2.3167971720485934, + 2.3463759940634703, + 2.3732528454295974, + 2.3977015645263875, + 2.419424716496846, + 2.4391257210056154, + 2.4581661721343564, + 2.4760834025730842, + 2.4948209717097534, + 2.5152157292469712, + 2.537726956003219, + 2.563045175832419, + 2.5923259488744463, + 2.6235803001400018, + 2.6547104375531503, + 2.6882468672845015, + 2.722868273296302, + 2.7577078965335047, + 2.7947002260250353, + 2.833303583593914, + 2.8722764445081204, + 2.913744128947523, + 2.9582002371524707, + 3.0043149595698164, + 3.054572368166239, + 3.109635545671697, + 3.169127382480864, + 3.233982551457134, + 3.305067027519865, + 3.381649352350055, + 3.462579502841436, + 3.548881480285042, + 3.6387510749962053, + 3.73422127659939, + 3.837246845655587, + 3.9438950955057437, + 4.055148513043715, + 4.175796563345039, + 4.299857145929124, + 4.427920028760938, + 4.564200864570259, + 4.706662589097586, + 4.845795057310785, + 4.990574450932911, + 5.141452335010133, + 5.2924638623179305, + 5.445462298161452, + 5.6047012143928665, + 5.7656655200045765, + 5.923161432890435, + 6.086715257279508, + 6.253030729176701, + 6.418953847168114, + 6.591517594838, + 6.767784638605135, + 6.939741504314393, + 7.117000458113039, + 7.300423866865699, + 7.485421635925301, + 7.672668670726102, + 7.867973639907908, + 8.064928215884052, + 8.261454641495268, + 8.456323783215645, + 8.653707009385208, + 8.853109363506482, + 9.052095269379818, + 9.251010569352694, + 9.455883497431737, + 9.664548108832998, + 9.86867958138228, + 10.072192968002337, + 10.283140091676069, + 10.493257173499961, + 10.697821767737025, + 10.911771274136825, + 11.127694008643468, + 11.338431291874018, + 11.555933018342088, + 11.77915157388375, + 11.999906705493894, + 12.22073278204977, + 12.450875494394165, + 12.677462465857127, + 12.89944884663247, + 13.131417460797836, + 13.365342006427221, + 13.59036767840702, + 13.819426994262399, + 14.052295347324945, + 14.27679261328338, + 14.505431181488275, + 14.74058925982937, + 14.96915917570018, + 15.197299561430436, + 15.424908144460323, + 15.64782793185962, + 15.86418333806608, + 16.081449408336358, + 16.302276766260317, + 16.520593847042477, + 16.73952000038133, + 16.966763792036904, + 17.187508805665516, + 17.402097786691098, + 17.62437166898507, + 17.849784911959865, + 18.067067204532794, + 18.288807239623967, + 18.519673597989403, + 18.741453074728906, + 18.960201003842627, + 19.187528970108, + 19.40603607314136, + 19.6295802469014, + 19.851471769792692, + 20.063892902360426, + 20.28284044547063, + 20.49506265498528, + 20.70484641453179, + 20.931845452406186, + 21.14380278638029, + 21.351556856093342, + 21.573710934054002, + 21.797372150281266, + 22.025143555702744, + 22.26162635728783, + 22.49555917009494, + 22.74108251568682, + 22.997379529708848, + 23.259893410872117, + 23.521739121800756, + 23.803526124778454, + 24.085909301716974, + 24.364358564513505, + 24.66952019053571, + 24.984296990340198, + 25.288991394864112, + 25.621334529400418, + 25.94923672774287, + 26.231807525533302, + 26.528803991800288, + 26.83558524180938, + 27.13251200071202, + 27.43363846792482, + 27.745063153098563, + 28.050495350322393, + 28.352985451389543, + 28.657802793457517, + 28.965318681392027, + 29.290414036766883, + 29.613305450592385, + 29.935893118228776, + 30.261392835932636, + 30.590872510173433, + 30.923368052022465, + 31.244988057728378, + 31.575803204180385, + 31.911151803658623, + 32.25018089339783, + 32.59480910699614, + 32.937627402566825, + 33.28794588439994, + 33.6279347381299, + 33.96190557936996, + 34.294610113578194, + 34.627339545147244, + 34.95052864881934, + 35.281680386612635, + 35.6130857622301, + 35.9290746696189, + 36.24852163663968, + 36.57422177102859, + 36.899631083515544, + 37.22588313597998, + 37.559215504866344, + 37.892777877735455, + 38.22551890017168, + 38.560538554247486, + 38.89158251079862, + 39.23302529975385, + 39.57820606213428, + 39.91884227485884, + 40.26306141062812, + 40.60937815640735, + 40.951666628629305, + 41.29318404025987, + 41.63657129801098, + 41.975595254807295, + 42.31734029198733, + 42.65353646987068, + 42.98831713780912, + 43.32888139048449, + 43.6655586896088, + 43.99368605046921, + 44.33142138232008, + 44.66215604052806, + 44.98490126596221, + 45.320020446725124, + 45.6423555683974, + 45.957346640807316, + 46.28348996674704, + 46.60040390395959, + 46.911337690664304, + 47.23368236810229, + 47.548562973696995, + 47.84594364853935, + 48.161893501649466, + 48.47966621412509, + 48.78104052066766, + 49.09550960412552, + 49.41365828497713, + 49.71392945487082, + 50.01885034497675, + 50.32869305739655, + 50.62525095914379, + 50.92564264333343, + 51.23246453113018, + 51.524620784434596, + 51.81150141540853, + 52.10062067544182, + 52.380974291115784, + 52.65774195484237, + 52.92921084593508, + 53.204554760149584, + 53.4783903496776, + 53.75658240531161, + 54.0366105377843, + 54.31404765242369, + 54.59787519797507, + 54.88992129520901, + 55.18350011196275, + 55.47300473071834, + 55.77115199960749, + 56.06407678115986, + 56.34957599191736, + 56.640500017847735, + 56.918919577569525, + 57.18651191609925, + 57.464796277014536, + 57.7489816803978, + 58.01228352171607, + 58.27448447590476, + 58.549261764877976, + 58.81966354190635, + 59.0810190597338, + 59.34550804391031, + 59.621904510986425, + 59.89243981498452, + 60.15054633062333, + 60.4146359330284, + 60.690927131716464, + 60.95117336000782, + 61.211926574230255, + 61.49175187232582, + 61.75862622972725, + 62.002538063312464, + 62.25130083747442, + 62.494978305394376, + 62.70413113581055, + 62.90684831747295, + 63.11819867225374, + 63.30876360557971, + 63.48255241562824, + 63.65652732608937, + 63.81188357849256, + 63.952223161903035, + 64.08384835040273, + 64.20228694366901, + 64.3058778678585, + 64.39431052265087, + 64.46181476053984, + 64.5138221164496, + 64.55037676437557, + 64.57202236022114, + 64.58122002040913, + 64.57357281798858, + 64.54846525516317, + 64.5066277721625, + 64.44855207527613, + 64.37828463181846, + 64.29050019600527, + 64.1837097623907, + 64.06820377617622, + 63.9359705388783, + 63.78154042851915, + 63.62139498865959, + 63.443128478639025, + 63.246074115359114, + 63.044716788422015, + 62.830994017778295, + 62.59475675268386, + 62.35145991094609, + 62.102827126482275, + 61.836469170239184, + 61.56052117465298, + 61.28350782294512, + 61.00211248774538, + 60.71319822773284, + 60.415230565970255, + 60.11068377896734, + 59.80297097306492, + 59.487662636397125, + 59.166261821624495, + 58.84489002582075, + 58.51493489560721, + 58.17376645681918, + 57.83577562934087, + 57.50159619448542, + 57.150206968316674, + 56.8098775057813, + 56.480478255854734, + 56.1337826236115, + 55.79212702638441, + 55.46529845716434, + 55.122389420158356, + 54.78620751035339, + 54.45562989734567, + 54.112611391115095, + 53.77183859158204, + 53.4346064857942, + 53.08624326957375, + 52.73705271285156, + 52.396702556113986, + 52.04201990757297, + 51.68831138692674, + 51.341335793576874, + 50.98392454616007, + 50.63082960851566, + 50.2845751874734, + 49.935157892616516, + 49.585502316420516, + 49.23450291787667, + 48.89043106822508, + 48.53718118140634, + 48.183106174437874, + 47.83290180765036, + 47.48342297947092, + 47.13267354616005, + 46.779444550330155, + 46.4290543350682, + 46.079316639733236, + 45.72484143705796, + 45.33074088809445, + 44.97905968459101, + 44.61934566425871, + 44.25404515186802, + 43.89440567514032, + 43.52880276659065, + 43.16432513341936, + 42.80109993962998, + 42.426443112120424, + 42.05458400887959, + 41.68869886475714, + 41.30406811282386, + 40.91732412143384, + 40.55184686255979, + 40.162958205166085, + 39.77007580965446, + 39.39551996242697, + 39.011968802918155, + 38.61155593306669, + 38.237991349744036, + 37.862498253284045, + 37.47087662406409, + 37.09438850324539, + 36.725218789343096, + 36.34145903615912, + 35.962788171007944, + 35.59742720821883, + 35.22314046889542, + 34.845470615037904, + 34.47077348983268, + 34.10078165992983, + 33.720052942159576, + 33.34048087728195, + 32.970353952554824, + 32.59422500184866, + 32.21704072487308, + 31.8485029905479, + 31.48400049576467, + 31.112191483180407, + 30.739825730803798, + 30.37312850259972, + 29.99985756267379, + 29.617963798496188, + 29.2467291159945, + 28.875955840739014, + 28.502309183512967, + 28.135940856742728, + 27.766807861272547, + 27.39749678995351, + 27.034658109848234, + 26.677225788696784, + 26.318530075613232, + 25.967085718463615, + 25.627262362385075, + 25.277091515812, + 24.93020802031985, + 24.58994780320747, + 24.23465416028736, + 23.88850758412448, + 23.548554735994852, + 23.202723948728426, + 22.86842250365915, + 22.538816273231063, + 22.205466008741364, + 21.883740759053744, + 21.559288904454927, + 21.224337741051304, + 20.91460446251487, + 20.59968776191551, + 20.264677799873517, + 19.954420542202634, + 19.642669745042046, + 19.314741441083697, + 18.998021408460353, + 18.688882635627362, + 18.36340240744392, + 18.052109997714656, + 17.76556782272875, + 17.47181725701148, + 17.182570274630805, + 16.904251916910184, + 16.62114939379875, + 16.33708929371887, + 16.059215464919923, + 15.775224779471078, + 15.492091854500226, + 15.202113729578942, + 14.903877747934894, + 14.602521465534105, + 14.301146836855489, + 14.004371042662179, + 13.704510927008922, + 13.404502075527272, + 13.106773714107563, + 12.810173788665038, + 12.50792605386516, + 12.204309055755756, + 11.901595980951306, + 11.597353718742553, + 11.29308072858805, + 10.994020537350567, + 10.699498462628563, + 10.400942490118092, + 10.107273838156436, + 9.819291714596416, + 9.53246740519458, + 9.246960777908608, + 8.967798298246185, + 8.687201947034684, + 8.406735056830456, + 8.129034611020845, + 7.84208767826444, + 7.552771676692099, + 7.270788312748914, + 6.948883786893604, + 6.647360760685175, + 6.352150079582912, + 6.054043535841086, + 5.747904281550427, + 5.450203041801112, + 5.153571962327716, + 4.845539280114005, + 4.547248750200525, + 4.254694092048906, + 3.950410623554936, + 3.653454874272269, + 3.361836168080324, + 3.0649956197929056, + 2.784864028950497, + 2.5030941040493673, + 2.2184336888929423, + 1.9506677645006707, + 1.6831380259049515, + 1.4099914836804952, + 1.148007225055149, + 0.8825854868148396, + 0.6091438528935407, + 0.3329631741793019, + 0.047078976871867295, + -0.2352447315031835, + -0.508756199989569, + -0.7853828673457621, + -1.0603026857941362, + -1.338679442750711, + -1.6007652894089202, + -1.847553644318416, + -2.1040329713990307, + -2.3418535145622625, + -2.5538629993803976, + -2.765133911770426, + -2.9701720576168875, + -3.1595598230605817, + -3.3382909620172394, + -3.504147407326207, + -3.6539679529776756, + -3.78939863520616, + -3.907272219931894, + -4.0066095925570115, + -4.091204898540712, + -4.158262119894641, + -4.206623367713327, + -4.236058617552424, + -4.246644353929049, + -4.239034035612159, + -4.213506574398528, + -4.171128279359379, + -4.113710327681287, + -4.0419241443501, + -3.9559111690968516, + -3.857513719772755, + -3.748090036784706, + -3.626855195420487, + -3.496670562598065, + -3.3563265848874084, + -3.205151737412957, + -3.0488316854022117, + -2.8854476964654956, + -2.7128724275487377, + -2.5379062116092443, + -2.3614629685122734, + -2.163055172621855, + -1.982818367233269, + -1.8038731289925154, + -1.6272554952898974, + -1.4532546008280691, + -1.278990090742027, + -1.1098506009116396, + -0.942136755407772, + -0.776576723408273, + -0.6158370508830519, + -0.4544020997355928, + -0.29942973559885255, + -0.1454188060245573, + 0.007936003205972264, + 0.15565305258712397, + 0.29457625258933406, + 0.4314142580302064, + 0.5683430981854892, + 0.694846679137476, + 0.8188379360171041, + 0.9358809833941538, + 1.0527320108236529, + 1.1671660272613753, + 1.2671443883084748, + 1.3640722172091344, + 1.4577010292601715, + 1.542415850180607, + 1.6223748629558545, + 1.699971255890312, + 1.7685763797422365, + 1.8275838981024881, + 1.8824069707097382, + 1.9315755100308414, + 1.9761177832561962, + 2.0184224135107582, + 2.058772295444945, + 2.090048449146628, + 2.118414284995719, + 2.1433218020241127, + 2.164499847176934, + 2.183624398856786, + 2.201745563914696, + 2.2191163862191376, + 2.2356112568211453, + 2.252205190590705, + 2.2696734386851425, + 2.2861436871219065, + 2.3028388082520315, + 2.320156656784082, + 2.3385900642083643, + 2.3592713493285324, + 2.381709319584404, + 2.4070750850405918, + 2.433536714213401, + 2.460323180625303, + 2.488334737445989, + 2.517470701544321, + 2.546629802554478, + 2.5761035809681236, + 2.605801430812296, + 2.6346378618305946, + 2.664230743227441, + 2.694549804929435, + 2.7258159022823096, + 2.759238603216797, + 2.7944993759776464, + 2.8341113639860507, + 2.878222183433039, + 2.9265506539557307, + 2.9817570389312267, + 3.0424355076743477, + 3.10706981078607, + 3.176268493362981, + 3.2518366092586892, + 3.3306555083175096, + 3.4167003836949985, + 3.5103863409690583, + 3.608075276617331, + 3.712465197951945, + 3.824986779880386, + 3.9414953622228603, + 4.063337721981724, + 4.189228453085594, + 4.32305494845731, + 4.4601335086936915, + 4.599612768708412, + 4.745724565432593, + 4.894244097721296, + 5.041885910037444, + 5.191764910987589, + 5.346624945272, + 5.502136993699961, + 5.66031023985374, + 5.827040420228186, + 5.993915138638752, + 6.163019081791028, + 6.340004190690086, + 6.51480791576448, + 6.693052797442919, + 6.876344788889039, + 7.061744999846965, + 7.249530507137674, + 7.442401885917925, + 7.6363173798228825, + 7.833347531175167, + 8.031939902077687, + 8.231630218605986, + 8.437204769860044, + 8.646940742133532, + 8.857529835015656, + 9.07108788216632, + 9.290443494699435, + 9.513642844314555, + 9.734015257909563, + 9.952667334600143, + 10.176895422656376, + 10.399408406976416, + 10.612948927801359, + 10.835322750369697, + 11.060327826084288, + 11.278301473239495, + 11.503489875629574, + 11.73523294520646, + 11.963468785348505, + 12.191789007678494, + 12.429478131732653, + 12.665443031503617, + 12.89579158070827, + 13.136067873538982, + 13.377967392787282, + 13.609060380777938, + 13.842656718036658, + 14.079282585095763, + 14.303025116185163, + 14.531193772188637, + 14.76343373257601, + 14.988364717265448, + 15.210401108907472, + 15.432194824916941, + 15.648898221378381, + 15.859666504257781, + 16.07159243459752, + 16.28587704705328, + 16.492741049637687, + 16.70271477593439, + 16.916959488029352, + 17.120388676395603, + 17.321872086130636, + 17.52598437352893, + 17.73069132213282, + 17.923541381424986, + 18.12415877679045, + 18.33055247009981, + 18.527339344950683, + 18.728619393999544, + 18.93229651117788, + 19.159708602880198, + 19.4082784942946, + 19.657013882891285, + 19.913688153069966, + 20.1810237350667, + 20.44925984654765, + 20.71943223986166, + 21.013255340780542, + 21.296880996082397, + 21.587008717105277, + 21.87669543522121, + 22.14614390357882, + 22.429699716773424, + 22.72867919352653, + 23.015124808272496, + 23.309689131058693, + 23.62072737599991, + 23.917317997509155, + 24.22728923897416, + 24.549557830741826, + 24.859284779918198, + 25.198737878170633, + 25.55124425892161, + 25.891361522185825, + 26.253819551000504, + 26.613487891441043, + 26.953390314574257, + 27.310638571378494, + 27.675098879004985, + 28.02257609685884, + 28.372024437456844, + 28.722782278393833, + 29.056779778170263, + 29.391639482158382, + 29.733978619258064, + 30.06954199813284, + 30.405578557387145, + 30.74290460478035, + 31.076644811849285, + 31.415921869142643, + 31.75168566214963, + 32.07880615187477, + 32.40706885545298, + 32.73851460197841, + 33.0676807176834, + 33.39832381666462, + 33.722583165076074, + 34.047658623891216, + 34.379238402257826, + 34.69422166225884, + 35.000045231143986, + 35.31662744158378, + 35.626168566857515, + 35.92233434856951, + 36.23308218134385, + 36.54449892409094, + 36.84456016976044, + 37.1509961306791, + 37.46708218572251, + 37.778666332331575, + 38.09528208590343, + 38.41604525917596, + 38.73530274120566, + 39.055625712890404, + 39.37255657030364, + 39.68694055810615, + 40.00704721386262, + 40.32652264170847, + 40.648259894550165, + 40.97597427139211, + 41.301638338918146, + 41.62584438215976, + 41.956297836934525, + 42.284245628285845, + 42.613262425546324, + 42.95240079702139, + 43.28646993462193, + 43.625021677518454, + 43.97328896243987, + 44.312702543278725, + 44.64813124991473, + 44.99439254626033, + 45.3277421467613, + 45.66063501705099, + 45.99941329622676, + 46.3335422645042, + 46.67401168335572, + 47.01662165443693, + 47.35177127067828, + 47.68824468473037, + 48.02732167165485, + 48.34825278519832, + 48.67520754566773, + 49.009570294261664, + 49.330733779477306, + 49.64803478144664, + 49.97272288173262, + 50.28626181294306, + 50.59636292421281, + 50.91164452188793, + 51.217231473830275, + 51.523743506303816, + 51.83079606383554, + 52.12537571031702, + 52.41455454927051, + 52.69755428166409, + 52.97307848949289, + 53.244803055412994, + 53.51315808323412, + 53.77889165725008, + 54.0491643707451, + 54.35163494380637, + 54.62137644655208, + 54.893989465334805, + 55.17536679309078, + 55.45710389409889, + 55.74015307895498, + 56.0220228048467, + 56.303164609917786, + 56.58000819492296, + 56.841236795067466, + 57.09245391592853, + 57.339838256342624, + 57.58516029644259, + 57.82775634694091, + 58.070779490401414, + 58.308651526869305, + 58.57167097058137, + 58.81437714424254, + 59.060321005464154, + 59.31221678831944, + 59.57476904619701, + 59.84574422526881, + 60.10556921586334, + 60.36630394592955, + 60.64091672994618, + 60.911254785589264, + 61.16707885205474, + 61.434349921802685, + 61.70112616186897, + 61.93388386434675, + 62.15830832844092, + 62.38125936047842, + 62.590587097629445, + 62.787725268168, + 62.97481626703805, + 63.16308213907139, + 63.34120860191085, + 63.50523215150407, + 63.665414522459955, + 63.81702987323637, + 63.95334470163948, + 64.07447428964709, + 64.17894658610709, + 64.26765538233062, + 64.33715010727487, + 64.38424431844649, + 64.41170181037019, + 64.42031773573312, + 64.40931520031016, + 64.38339297026431, + 64.34336667198134, + 64.28737658398609, + 64.21855403422586, + 64.14121152797942, + 64.04995761227521, + 63.94846791151717, + 63.838660338765, + 63.71870140813576, + 63.58564694001701, + 63.43286502924833, + 63.26756956130647, + 63.089021454444065, + 62.89696735199743, + 62.69109830514015, + 62.475849872656646, + 62.24723760717158, + 62.00403832883274, + 61.74744926263506, + 61.48441603100128, + 61.21147438509104, + 60.92554547200103, + 60.64556168698664, + 60.35556131366098, + 60.05528774603434, + 59.761106324338954, + 59.45953567556409, + 59.14674387373742, + 58.83877987674616, + 58.52537351931939, + 58.19696747819619, + 57.87673206832023, + 57.55904407515568, + 57.219522834317935, + 56.89825560150076, + 56.583692262369006, + 56.247382950278954, + 55.925194275975464, + 55.61661738404746, + 55.287665670641275, + 54.97358775518657, + 54.667044557843184, + 54.34926065592826, + 54.033403777056186, + 53.72032698465734, + 53.39942408763453, + 53.07425797607542, + 52.76026576865347, + 52.43499851719745, + 52.1044872768535, + 51.77589236535425, + 51.44234818902297, + 51.10348480799015, + 50.77061054705971, + 50.43943035416799, + 50.105447993078876, + 49.775680140952204, + 49.437693937288984, + 49.109475016229915, + 48.774063273912645, + 48.44174250821862, + 48.11031287371997, + 47.780591614667, + 47.455178389408125, + 47.12345403343129, + 46.79250829325339, + 46.463096924711756, + 46.1302104268118, + 45.79628426756038, + 45.46087308387347, + 45.12707494978503, + 44.78291080964345, + 44.435197543313826, + 44.095601761189705, + 43.74644247020221, + 43.398053080714256, + 43.05685402972333, + 42.707021234983905, + 42.35524805506865, + 42.009044387462, + 41.65243165458508, + 41.276103231129696, + 40.9187023488142, + 40.5627451827555, + 40.18094692538712, + 39.81602462825786, + 39.4544890754482, + 39.075386646568006, + 38.70226880539241, + 38.3415785656529, + 37.97676501433701, + 37.60772571361131, + 37.250223049724525, + 36.88744324348057, + 36.52416454726091, + 36.1573556895985, + 35.79788148487995, + 35.437265081393555, + 35.07271719105067, + 34.70233914650486, + 34.334966911426164, + 33.96279699298524, + 33.57711933124139, + 33.20363930786742, + 32.83230262301181, + 32.460658625022646, + 32.08325053537434, + 31.71958169915117, + 31.357203914839488, + 30.990154777772926, + 30.631982239623195, + 30.275221666848807, + 29.915767642342555, + 29.55390361714718, + 29.197115634246998, + 28.838005033417023, + 28.47869388507307, + 28.13208967675225, + 27.77559158249143, + 27.421637809516504, + 27.090678679335795, + 26.75922066017794, + 26.43484794550527, + 26.11289662642745, + 25.79380377776938, + 25.47813120634998, + 25.164360541699196, + 24.847550507204833, + 24.534393432863162, + 24.220401011247223, + 23.90615233246471, + 23.589665253530022, + 23.273895528473595, + 22.971481724084825, + 22.665888937391326, + 22.3620898886782, + 22.069296232048632, + 21.77440371495339, + 21.466856629222264, + 21.17859792836629, + 20.8934541543908, + 20.58147048470993, + 20.285748877176808, + 20.001366350635404, + 19.69549193285993, + 19.395772270006105, + 19.102822298479825, + 18.79866540471908, + 18.49086458184024, + 18.18718747554513, + 17.87562827174696, + 17.561654080135373, + 17.25428736220401, + 16.943385909834873, + 16.635600645459125, + 16.328601752378894, + 16.022285516326736, + 15.713355480669735, + 15.406459366001444, + 15.098179998114757, + 14.788261722135921, + 14.480811970868182, + 14.168821267560622, + 13.861490918040086, + 13.552445228312457, + 13.241443743478781, + 12.931410145368236, + 12.621438580723158, + 12.308900177709276, + 11.991856791088074, + 11.677526845036505, + 11.368176690497656, + 11.05392183631032, + 10.741464037040421, + 10.439526008169324, + 10.136818386414463, + 9.82423644278, + 9.531400298275525, + 9.238686371846137, + 8.941403335004782, + 8.65284802362914, + 8.370242659390138, + 8.090755133450198, + 7.810471819959358, + 7.5290850443663935, + 7.255905312909552, + 6.981271089469676, + 6.696914938425842, + 6.4181863050356345, + 6.146481325295194, + 5.85483052076996, + 5.570950955358325, + 5.297348884398934, + 5.001466191033476, + 4.702468465076711, + 4.418485385786011, + 4.116906583262891, + 3.8174808038538623, + 3.5235553830862942, + 3.2118632499177506, + 2.9281086942201098, + 2.6440876629142287, + 2.344325939053991, + 2.0635779600180695, + 1.787671719069037, + 1.4993269583573747, + 1.2207628985657388, + 0.9454794304059446, + 0.6640958804600767, + 0.3877002089322421, + 0.11448444930974705, + -0.16578620518746093, + -0.44750527714441424, + -0.7258070032432261, + -1.0046909442100316, + -1.2832724040354047, + -1.5631608335006302, + -1.839274903874311, + -2.10599621587619, + -2.3694675277233017, + -2.62343893987978, + -2.858403956492617, + -3.090102958119805, + -3.3069641079040974, + -3.507229477288608, + -3.6938382521769215, + -3.8629814663577817, + -4.01420132205265, + -4.1466584356736265, + -4.259877706509005, + -4.352527189824512, + -4.422964230685253, + -4.473565063110901, + -4.502183007140632, + -4.507021577830109, + -4.489283337148372, + -4.45174440211488, + -4.397050804131656, + -4.323598131913655, + -4.231769913016596, + -4.124768055855302, + -4.000782801956151, + -3.848806574422038, + -3.7008431642453847, + -3.542712038947353, + -3.378535522961384, + -3.2077932292354094, + -3.030690460174938, + -2.8519001098794727, + -2.666681213026716, + -2.477801972577572, + -2.288094039386811, + -2.0954870850756433, + -1.902136836077668, + -1.7092534902537027, + -1.5183759989546781, + -1.32893877896922, + -1.1404759348918994, + -0.9554250517824684, + -0.770610521851054, + -0.1285582442863698, + 0.043080907309530216, + 0.211280767432397, + 0.37327933021129, + 0.5247876104488909, + 0.6714241109417203, + 0.8148578400793581, + 0.9457752361654689, + 1.0699833089646602, + 1.1874050240947238, + 1.3062525644859002, + 1.4148382096503505, + 1.5101383386416976, + 1.6047030472306796, + 1.6933293187395781, + 1.7742607545942723, + 1.8534355266406306, + 1.9285556499646628, + 1.9949586429074053, + 2.057049811045084, + 2.117137136923471, + 2.1715376262881505, + 2.224645268626372, + 2.2751400325408344, + 2.3194737710752404, + 2.3609646532785145, + 2.3993493477777537, + 2.434847778227501, + 2.4667596720468703, + 2.496246389384398, + 2.524205932018945, + 2.5508374102446174, + 2.5768897875550767, + 2.603334615586056, + 2.627617746830503, + 2.651458920424354, + 2.675565875821827, + 2.699632969712007, + 2.724885295219408, + 2.7514870422184745, + 2.7805366985599824, + 2.8100202298576566, + 2.839351985580052, + 2.870126952080361, + 2.902052472474704, + 2.934071944572666, + 2.9667649677538774, + 2.999161281092309, + 3.033247502509151, + 3.069458790047973, + 3.109101654141103, + 3.1521991926926627, + 3.1984060810843857, + 3.2513735416748735, + 3.3096335977130296, + 3.3721912004533228, + 3.442704234125669, + 3.518800508446662, + 3.598955450020016, + 3.6852691404136575, + 3.776690115407541, + 3.873897199025853, + 3.9809590777696595, + 4.093369901196933, + 4.21073543806722, + 4.337228720660737, + 4.467472306282675, + 4.600399174940791, + 4.7378920080515625, + 4.8818043434231715, + 5.025019052801778, + 5.174247070572004, + 5.328205564256003, + 5.479368840755798, + 5.631256669496028, + 5.788513887931863, + 5.948897262006386, + 6.10670218711256, + 6.26844904483016, + 6.432903803230794, + 6.596779259376989, + 6.7669267608152985, + 6.951513019733041, + 7.121582061617247, + 7.296303722578637, + 7.471446604902217, + 7.648390856797305, + 7.8329936775282905, + 8.020794966136831, + 8.209632808845917, + 8.401417620202345, + 8.590646064052626, + 8.783114896662422, + 8.978829742472467, + 9.175300105333713, + 9.375093784722608, + 9.576483388082313, + 9.776822687641996, + 9.974478826265862, + 10.16913283903571, + 10.363268687008691, + 10.55903746225986, + 10.75563130245319, + 10.945306668273254, + 11.135763190050596, + 11.332323488480347, + 11.525328914372237, + 11.720578016653537, + 11.926693929229051, + 12.135399727870412, + 12.342714717150432, + 12.55003234180756, + 12.761801265083607, + 12.97256146778967, + 13.179128127067901, + 13.387694609538476, + 13.597709724968675, + 13.80361797596977, + 14.006064805723163, + 14.207421030004635, + 14.406475207674958, + 14.601841720278095, + 14.793749819797004, + 14.987798478702754, + 15.181954233018319, + 15.372252885129878, + 15.558553037905114, + 15.745354404116084, + 15.931025015334148, + 16.112133567599226, + 16.29169380601665, + 16.474231645900765, + 16.65673077203514, + 16.838598653819968, + 17.024090442972696, + 17.215763953323627, + 17.40575160280916, + 17.593940114722514, + 17.78483760605761, + 17.981528288357495, + 18.182422671778454, + 18.374776814112163, + 18.581111160182836, + 18.79362259697671, + 19.002776113219582, + 19.21626970605701, + 19.43112870827955, + 19.65102080898132, + 19.878853568831616, + 20.10312546864239, + 20.33456798064165, + 20.571455677987018, + 20.805307917268557, + 21.045403500637494, + 21.298626572310877, + 21.53535576431192, + 21.787166760774234, + 22.04704548994729, + 22.30130520146322, + 22.574917940685502, + 22.846884374467052, + 23.11244824903883, + 23.394736111510714, + 23.676559415391722, + 23.953561991223374, + 24.250244848173462, + 24.540664243181794, + 24.833706554567964, + 25.156073221083844, + 25.473212534749933, + 25.796484527816077, + 26.13727266230354, + 26.460167230968434, + 26.77512580126815, + 27.108708116478457, + 27.438704853826536, + 27.759939243778156, + 28.097005722458324, + 28.43352407824126, + 28.759042039978237, + 29.089389232226157, + 29.420449816116722, + 29.74983119488025, + 30.08212078675594, + 30.411269707900978, + 30.742944489098743, + 31.079775794747977, + 31.415306515442328, + 31.741820418039623, + 32.07149075857679, + 32.40361086995983, + 32.73287821237294, + 33.05788248022835, + 33.377945876076446, + 33.703491287061006, + 34.02881086933761, + 34.336350875136155, + 34.63920540415571, + 34.95101710678149, + 35.252506576967846, + 35.54453913345566, + 35.85123718330825, + 36.1556024863718, + 36.44855841864565, + 36.750834738479114, + 37.06178859755466, + 37.36669465341993, + 37.67828627342124, + 37.99722984607634, + 38.3128101331543, + 38.63145517761055, + 38.94999809789752, + 39.262558010339916, + 39.579606480905696, + 39.900656383320424, + 40.21583267579129, + 40.53472068673256, + 40.85465562257616, + 41.171871203786324, + 41.486037357245635, + 41.80341437120019, + 42.115184424916826, + 42.432267817800195, + 42.74869006613391, + 43.06151791935443, + 43.3820080676286, + 43.70140529186662, + 44.013298163509596, + 44.328170401408954, + 44.64674426514315, + 44.947785521041396, + 45.25663632901949, + 45.56604597424447, + 45.86151513692612, + 46.1652166857148, + 46.47234999090252, + 46.76795480259499, + 47.06580954138513, + 47.369754222452364, + 47.66790276213002, + 47.955036340077136, + 48.25304046096536, + 48.55028755018086, + 48.84124349388114, + 49.13120357369446, + 49.425155662794786, + 49.71207103834986, + 49.997415742459815, + 50.28675152243265, + 50.57106337226466, + 50.851292024285996, + 51.13648119935449, + 51.417560945870974, + 51.69166214349666, + 51.96406627724454, + 52.22926991177055, + 52.48582986037931, + 52.745453668301394, + 52.99701452760204, + 53.24552141987308, + 53.4978257580014, + 53.7494697455108, + 54.00373136573797, + 54.255545635500454, + 54.50935013800976, + 54.770800669626155, + 55.036105044860285, + 55.30578103475355, + 55.57712629270071, + 55.84841517886736, + 56.121305819378854, + 56.396127182219146, + 56.66480154268678, + 56.92590628816359, + 57.18698185333048, + 57.44827129411211, + 57.71011908320546, + 57.963999775378376, + 58.218160972638955, + 58.47851432665823, + 58.73886674078835, + 59.0018961803807, + 59.25945908560965, + 59.52850899544941, + 59.7983985531829, + 60.06082253745904, + 60.325324743604384, + 60.603310869586814, + 60.87044228018748, + 61.12857127677023, + 61.40259509282569, + 61.67108245307185, + 61.91466944388023, + 62.15701613438367, + 62.39918205440036, + 62.61468586808511, + 62.814890921539984, + 63.02869703724511, + 63.23373607245903, + 63.41827901302412, + 63.60299309694622, + 63.775328972893604, + 63.92889207470187, + 64.07696865776536, + 64.21234997683516, + 64.33590342676878, + 64.44841181662521, + 64.54345291530073, + 64.61930888948838, + 64.6804588488602, + 64.72498316710659, + 64.75503192542301, + 64.77423687634494, + 64.77803140213041, + 64.76324698744891, + 64.7316474879003, + 64.68467452846008, + 64.6238135747295, + 64.54563229844479, + 64.44741475264918, + 64.3356181562903, + 64.20980251223435, + 64.05914493782261, + 63.89264825264366, + 63.72007165595962, + 63.522447573320285, + 63.31200360543133, + 63.09432787491209, + 62.85875514113911, + 62.60149057913376, + 62.33823598972425, + 62.06428886562031, + 61.77172285159672, + 61.47355105704766, + 61.17496468507821, + 60.87045058716934, + 60.55938185071553, + 60.24397081513541, + 59.93095284858972, + 59.61728220176726, + 59.299333939218656, + 58.97707926350325, + 58.65792783521924, + 58.33271154277494, + 58.0012539725055, + 57.67455351559774, + 57.35069668912867, + 57.0110033390398, + 56.682707863948245, + 56.36397335195614, + 56.03032003075439, + 55.70119001727611, + 55.386535174913206, + 55.05762540355066, + 54.73099797658368, + 54.413186029836865, + 54.08440679283176, + 53.73343743190345, + 53.37357244061748, + 53.004494568369005, + 52.62954228046007, + 52.26446093215443, + 51.8915976868798, + 51.510535483981364, + 51.13531867866375, + 50.75821421464184, + 50.37417727794913, + 50.014487343204124, + 49.669224905922185, + 49.321863973983824, + 48.97802674707963, + 48.62923138335952, + 48.28707355315305, + 47.93763151574009, + 47.5872907197428, + 47.24000687906527, + 46.893839862634835, + 46.53587658051037, + 46.163590570402484, + 45.79241563967316, + 45.42317540460844, + 45.04551180986331, + 44.66978917424429, + 44.29262343794187, + 43.91757027134529, + 43.531587538619334, + 43.14696794743754, + 42.78206108151733, + 42.41824049184275, + 42.060184243998044, + 41.70452003079957, + 41.33805206358612, + 40.9766109893554, + 40.61983341924642, + 40.24887535369847, + 39.87223229958563, + 39.519149902919985, + 39.18455634113386, + 38.860337834576384, + 38.55863130316872, + 38.25385652507213, + 37.934972214089946, + 37.63300200201469, + 37.3396025668475, + 37.03070272139799, + 36.7277037174502, + 36.43124540575485, + 36.10315588004389, + 35.76046113590517, + 35.41316425324658, + 35.07222237259091, + 34.731873722071725, + 34.387032236592475, + 34.036139478930146, + 33.69462576680907, + 33.35530418497045, + 32.99712842178764, + 32.65920215189999, + 32.333351840655396, + 32.0051779050895, + 31.672771298385804, + 31.345114919508905, + 31.023647925277686, + 30.69561782251355, + 30.372857957424703, + 30.04455318675462, + 29.716530650772686, + 29.376775009459283, + 29.032727437247416, + 28.694625437234627, + 28.349447040615075, + 28.011036457098466, + 27.678311777443966, + 27.340469359712845, + 27.009506139351178, + 26.681481223625706, + 26.35123343731874, + 26.030749965565633, + 25.703695028905464, + 25.3729809368927, + 25.05138638577504, + 24.72510824817708, + 24.39159632548782, + 24.06553639184264, + 23.740206121656943, + 23.40739769660998, + 23.081261657381123, + 22.763948496106984, + 22.434754638579886, + 22.11865931239052, + 21.807961662977746, + 21.4905386192929, + 21.164729629764402, + 20.862205917135583, + 20.557598524940502, + 20.22856773223279, + 19.92575997577919, + 19.6153881903489, + 19.286986859541866, + 18.97106000671142, + 18.65889334937452, + 18.33971068441129, + 18.018534079578565, + 17.705214573794848, + 17.385735385576314, + 17.068365578110665, + 16.757617040072198, + 16.4496460625785, + 16.145614271502176, + 15.843183457518938, + 15.539724258363853, + 15.23081033491441, + 14.928168044593754, + 14.619350352711118, + 14.312415204065331, + 14.004900876356503, + 13.691505397129548, + 13.382761226417156, + 13.072808436633192, + 12.75617255762379, + 12.440687864840086, + 12.126135942931526, + 11.807694599494935, + 11.481563043217621, + 11.160286642289627, + 10.843404375716958, + 10.519724274272413, + 10.197474213686554, + 9.885910478792159, + 9.576727165359841, + 9.255507127598605, + 8.954497568968769, + 8.656825556729501, + 8.35151396705426, + 8.061474395638424, + 7.7724433786617295, + 7.479411741392958, + 7.188095097181444, + 6.89573277042006, + 6.601306178114268, + 6.311113766360433, + 6.012875205586886, + 5.711826949516217, + 5.418867168237042, + 5.11094740753219, + 4.812014104697031, + 4.518557033966874, + 4.21565271665293, + 3.9175057084956775, + 3.6254461948470915, + 3.331545966540914, + 3.0376112132400044, + 2.7496280170382965, + 2.458511058356632, + 2.1776750685582287, + 1.9023324270350852, + 1.6219873136478071, + 1.3515619052253622, + 1.085310220601021, + 0.8132238274266218, + 0.542831482048225, + 0.27315570468110895, + 0.0028421577680153853, + -0.27049276703125885, + -0.5463268248329006, + -0.8262004551646298, + -1.107879395286346, + -1.3916557107149121, + -1.6774387069881023, + -1.9574536540348448, + -2.2414835550731893, + -2.526726733690335, + -2.796098289993876, + -3.0485159716884143, + -3.3022453823800215, + -3.545904518964392, + -3.7680426080445755, + -3.9822654167228353, + -4.182143455408361, + -4.362342772072077, + -4.525006759792942, + -4.667669520894381, + -4.786895394067779, + -4.882482773350822, + -4.9567380382589485, + -5.00760632093183, + -5.033904877646841, + -5.035950736923687, + -5.015924590660966, + -4.977009753510907, + -4.918364667515823, + -4.840335445178749, + -4.7456233965048025, + -4.633561589034956, + -4.505079616704708, + -4.36172030627776, + -4.204324694798168, + -4.036262663020632, + -3.8583288185661546, + -3.6709480583204877, + -3.480061577759506, + -3.282265203079415, + -3.079398118769766, + -2.87609039202851, + -2.6689610642012136, + -2.4603357019972623, + -2.2549151712934425, + -2.0555541820113747, + -1.8414780652124227, + -1.651346634081261, + -1.4682549265943972, + -1.2889642340245302, + -1.113857587858442, + -0.9438890907116492, + -0.7775191634099369, + -0.6157830205941762, + -0.4605198406816952, + -0.30980998000948873, + -0.1681397575337732, + -0.03536969681095837, + 0.0949241979294292, + 0.21648296179149223, + 0.33224419728985344, + 0.44184639524177516, + 0.5530670602380464, + 0.6552431131693358, + 0.7450458870719152, + 0.8349279146221444, + 0.9197564848703558, + 0.9981466064915008, + 1.0757093210853481, + 1.1498858268519423, + 1.2157172484173244, + 1.277918760588237, + 1.3369809429183377, + 1.3913095375182345, + 1.4445619806903216, + 1.4930199416312082, + 1.5353990216939157, + 1.57393914736842, + 1.6094500826339255, + 1.640371474278643, + 1.6681913617370199, + 1.6941189600356357, + 1.7183310280475588, + 1.7415256307133822, + 1.7652994469640837, + 1.7876405491265939, + 1.8093238050473628, + 1.8308054348159348, + 1.8520438795362315, + 1.8748257790396303, + 1.8996520904616516, + 1.9262057756298006, + 1.952449525353578, + 1.979242520411555, + 2.008259301776948, + 2.0384574799230704, + 2.069874562285853, + 2.1033989223984486, + 2.139669610737211, + 2.178847733484213, + 2.2224087485069375, + 2.2704188602062985, + 2.321978765945034, + 2.3805472836395496, + 2.451333063077684, + 2.5204186862095352, + 2.596175004887099, + 2.6769282243746972, + 2.7612674531780805, + 2.850935968758286, + 2.944825825949513, + 3.0466485157760412, + 3.1548488331499427, + 3.2655988413770833, + 3.3841870985236198, + 3.5110067392985895, + 3.6403081450036745, + 3.772111482310876, + 3.91300797460865, + 4.053585243976242, + 4.196203948618943, + 4.346386942022684, + 4.4964075473362435, + 4.6438185147866395, + 4.797364815007405, + 4.957656078701385, + 5.113551060641826, + 5.277450170411978, + 5.447738805582302, + 5.61868728094935, + 5.792274115754509, + 5.970106560387088, + 6.1457914048665065, + 6.3240470581112325, + 6.5038232997804295, + 6.684811613747742, + 6.867542189719006, + 7.0527788657473085, + 7.23832605270834, + 7.4271045363039025, + 7.61488157945303, + 7.803947766682915, + 7.997218280776169, + 8.191109069136724, + 8.386710094937602, + 8.584029781015781, + 8.784563043070957, + 8.98616378095811, + 9.186300524490916, + 9.384675712138709, + 9.58578425900947, + 9.785127603053741, + 9.977131089883029, + 10.17423205860926, + 10.375199026198787, + 10.589069677872354, + 10.787897957613902, + 10.9944989318322, + 11.199313719525836, + 11.40220889785868, + 11.611422327244709, + 11.822203056455006, + 12.028078159659541, + 12.236382887514369, + 12.448917898577703, + 12.656943496096835, + 12.86293268568178, + 13.068496729897587, + 13.273911901456566, + 13.476843235127044, + 13.677405638669933, + 13.880626236854065, + 14.083914035531546, + 14.281469296468464, + 14.477584788810997, + 14.696795871184278, + 14.891196227875946, + 15.082311130722983, + 15.276633135204207, + 15.472227614634006, + 15.667924286693639, + 15.866672791542968, + 16.0723923531363, + 16.277184547378898, + 16.48010689593563, + 16.683861003762978, + 16.89619857368472, + 17.108556511388578, + 17.315968162417718, + 17.539772148344706, + 17.765361978006677, + 17.990712281871108, + 18.21992972308881, + 18.451895868100554, + 18.68826151201862, + 18.9436482275444, + 19.19867073009613, + 19.463662386025156, + 19.730008903195234, + 19.9952377679208, + 20.276325961206325, + 20.541583971021918, + 20.819617418066056, + 21.104824436489302, + 21.38463648974693, + 21.66152259805987, + 21.932805429587546, + 22.199802235368654, + 22.482130779534508, + 22.761252586310977, + 23.03887936669711, + 23.334516634642657, + 23.61941361492147, + 23.911167285421456, + 24.228285841106565, + 24.53476624296535, + 24.855426896910537, + 25.18760203848001, + 25.49634142742295, + 25.802876757165475, + 26.12767599834626, + 26.448340980539754, + 26.758791512708367, + 27.084390931040822, + 27.40786261649449, + 27.731069968144634, + 28.05683599811312, + 28.3814308398038, + 28.702588021869904, + 29.024447450977327, + 29.342072286186745, + 29.65736677249564, + 29.980166530694827, + 30.29936164656833, + 30.609351396683735, + 30.942218991646982, + 31.274985758940357, + 31.59873538561554, + 31.917679388804146, + 32.22637499903647, + 32.53261266046398, + 32.84957140299829, + 33.1537604937998, + 33.45175328611491, + 33.75321001672992, + 34.04505669586066, + 34.3315081975387, + 34.61734665505737, + 34.91373649998367, + 35.20612154712153, + 35.4972385862957, + 35.79638421038561, + 36.10143582856517, + 36.40616431502931, + 36.7166564837261, + 37.03092016042375, + 37.34524010017537, + 37.66244537940462, + 37.98099835555076, + 38.29525213913023, + 38.613917564197976, + 38.93655431141074, + 39.25684937883945, + 39.58090054663728, + 39.908278426885154, + 40.2359741012518, + 40.56312289221807, + 40.89323347893464, + 41.252771217355594, + 41.583297146888995, + 41.90962386932896, + 42.22992667364179, + 42.55172726340942, + 42.86374484428655, + 43.163666724218984, + 43.4657626112107, + 43.76206757207292, + 44.03554399500987, + 44.3155751391994, + 44.59400240126925, + 44.85395319498643, + 45.12284975288024, + 45.39909856508403, + 45.66769236792282, + 45.94006227356499, + 46.21462418755764, + 46.4915005790355, + 46.77438105175335, + 47.04990057016058, + 47.32688896385801, + 47.6175106516778, + 47.90280342864727, + 48.18506541471182, + 48.47461299275151, + 48.761709218167425, + 49.043855530041554, + 49.333408220082184, + 49.624954836179626, + 49.907927570110175, + 50.196870979637715, + 50.49018285919311, + 50.77470018970976, + 51.05728309383378, + 51.33949904509417, + 51.61068049658557, + 51.88334215765688, + 52.153806801541364, + 52.42080888823905, + 52.692224989140016, + 52.96637536516427, + 53.24359290174978, + 53.51893086188829, + 53.799865792365004, + 54.088940690466885, + 54.38145622363204, + 54.67559382248205, + 54.972990652781654, + 55.26971689965987, + 55.567887137864005, + 55.861405261790225, + 56.146119876549264, + 56.43012315788755, + 56.71446933117696, + 56.996859523816624, + 57.26842312183835, + 57.544300551709576, + 57.825632761545144, + 58.10239128015138, + 58.375800451310475, + 58.65452129529378, + 58.93909611047069, + 59.21085667673081, + 59.47113500778839, + 59.740987264165724, + 60.00288489650001, + 60.2411062117452, + 60.489616959428595, + 60.74440561630401, + 60.97948612363302, + 61.20377689502732, + 61.42669067933879, + 61.644217562591095, + 61.84154689449571, + 62.02494818540991, + 62.216494455226645, + 62.40301496814696, + 62.5729062602609, + 62.739733953662665, + 62.90127769126194, + 63.0479087832785, + 63.188862066127065, + 63.32009926727753, + 63.43892000765799, + 63.548062032563706, + 63.64495326883894, + 63.723300638418195, + 63.78756769276339, + 63.83887868470011, + 63.87464820900632, + 63.90015377300029, + 63.91353245152039, + 63.91048662090791, + 63.891427781412744, + 63.85912787946155, + 63.813384867752916, + 63.75416329934468, + 63.67813296736693, + 63.584576370320185, + 63.482783152394894, + 63.36265994441006, + 63.22093511317302, + 63.07142173192214, + 62.90890336399922, + 62.72142432038193, + 62.52867142173112, + 62.323333738872115, + 62.09941765674898, + 61.85980415567179, + 61.61067513099217, + 61.347178835717614, + 61.06974311268001, + 60.78741520466235, + 60.497830705641256, + 60.20279873773858, + 59.89873348432303, + 59.585043621096226, + 59.266936513389, + 58.948489803259854, + 58.621791337080595, + 58.28971966682808, + 57.962389710684064, + 57.62597298102287, + 57.27909752193891, + 56.936167214092045, + 56.599916639903, + 56.26025818730859, + 55.923789485388916, + 55.601350547949295, + 55.27176884837666, + 54.93680527794924, + 54.62045518974739, + 54.296942341618085, + 53.96884906228317, + 53.65506611243988, + 53.33251378359047, + 53.00009929624337, + 52.676468408131186, + 52.34445179465412, + 52.00681219218575, + 51.67495333296684, + 51.34402544088789, + 51.001131750417386, + 50.66352177516216, + 50.328657755750555, + 49.984471850724496, + 49.649447251285785, + 49.319524842901195, + 48.98736591985296, + 48.6577174415375, + 48.32583472324404, + 47.993326969072214, + 47.66618385550341, + 47.33024882085921, + 46.996015374056576, + 46.6625747941048, + 46.3288997901906, + 45.99449743748959, + 45.65820973178802, + 45.32367401442467, + 44.98701469094136, + 44.65087242754459, + 44.3128161997339, + 43.97490572725108, + 43.60540030037656, + 43.2693660002701, + 42.9533266225585, + 42.63806078501144, + 42.32052615358233, + 42.0095836811218, + 41.693147034220765, + 41.37030589200015, + 41.056625342652836, + 40.74007253894454, + 40.40955698480483, + 40.07238619335842, + 39.73983985942109, + 39.39260096861668, + 39.029058991457305, + 38.68573293410243, + 38.33982956594653, + 37.9793949940433, + 37.63018870182943, + 37.29310655753665, + 36.95160633019749, + 36.60880867832948, + 36.293045043930555, + 35.97320318330131, + 35.65843250820487, + 35.34143487093785, + 35.023269264007865, + 34.71491328954702, + 34.404730125534805, + 34.08880522075607, + 33.77287508607854, + 33.464978895564755, + 33.13532789499079, + 32.79521185152687, + 32.470538905825705, + 32.142421903556844, + 31.816023894313595, + 31.489715051373466, + 31.162633798062817, + 30.844728987655056, + 30.520512393006193, + 30.200186095790748, + 29.874214475069444, + 29.546783390008734, + 29.22024519972762, + 28.88660565295937, + 28.556615940743562, + 28.225122145310184, + 27.887892089126876, + 27.55833359518771, + 27.225281584961564, + 26.880269079369786, + 26.546483679129313, + 26.211752975406316, + 25.872264985783026, + 25.54079640197197, + 25.206617009471472, + 24.87929742152391, + 24.54970022173698, + 24.21830077512884, + 23.89078380024956, + 23.55858934148418, + 23.229282320037715, + 22.90869003444346, + 22.582503646843886, + 22.259829266745648, + 21.951962768662916, + 21.636761446826245, + 21.325235204922894, + 21.027892349667027, + 20.71510396184291, + 20.400388442243187, + 20.10406009212683, + 19.788973839438082, + 19.465898738205222, + 19.16521682464404, + 18.853977589852196, + 18.533844973163838, + 18.225560236998408, + 17.916390066821002, + 17.593149552008764, + 17.279735159101676, + 16.96738557844563, + 16.61353796299852, + 16.300786009462797, + 15.993030516912416, + 15.681375942368238, + 15.370879265711897, + 15.064397930064088, + 14.749228817483711, + 14.439039644775507, + 14.124585889320837, + 13.804131998714254, + 13.481147050734426, + 13.157733911860888, + 12.834151570500785, + 12.505922526550986, + 12.175900497336459, + 11.84838349465872, + 11.519249127080254, + 11.185757644129255, + 10.852782421353982, + 10.525000382206137, + 10.192836056464515, + 9.866128307061684, + 9.542051026352523, + 9.217309348473334, + 8.895321172734855, + 8.578402333638502, + 8.26170217041076, + 7.939481330769564, + 7.627377744402807, + 7.3177884251394145, + 7.010961355341202, + 6.70668843031132, + 6.403785089924659, + 6.10947597925163, + 5.814662071230183, + 5.519360755593465, + 5.225755566271759, + 4.941538545824498, + 4.654884637718361, + 4.38037694029395, + 4.120549845484478, + 3.8539538181153965, + 3.579046402777778, + 3.313308735996936, + 3.0528396543980385, + 2.7771352753989516, + 2.50866923525166, + 2.2483543666163492, + 1.9711689430605315, + 1.6920598186618878, + 1.418285367364076, + 1.1352485535177619, + 0.8652150394627393, + 0.5996267110920918, + 0.3251281950151414, + 0.053165143872573595, + -0.2163908265782503, + -0.4892134221579513, + -0.754967675341181, + -1.016081762235151, + -1.2804261208534857, + -1.5432177157655607, + -1.8140142664177707, + -2.0899060194822106, + -2.36329393983016, + -2.6378118023299026, + -2.915525029592267, + -3.1890168242133385, + -3.454539702908877, + -3.7163576609103215, + -3.975675678123687, + -4.2169925406172375, + -4.442689768247701, + -4.658572553725139, + -4.852573105338675, + -5.030485206627612, + -5.190462235832259, + -5.3259049753047725, + -5.443858062965672, + -5.538424480407576, + -5.609667089114421, + -5.66232695645199, + -5.694878036607654, + -5.704945724777047, + -5.694117560591922, + -5.664957132654981, + -5.619711295630675, + -5.556593805077401, + -5.475222126440451, + -5.379112331432588, + -5.267802947825157, + -5.141050971444225, + -5.0015617749878665, + -4.8494632509374025, + -4.687618016712599, + -4.517439685474579, + -4.338229332376084, + -4.153840410571848, + -3.9630722745112554, + -3.762739198445909, + -3.559114993832612, + -3.354593878788111, + -3.1470325361190508, + -2.9382350311469287, + -2.7327155931539, + -2.5306624485787, + -2.328507542270429, + -2.132728645912017, + -1.9398885568742124, + -1.7454978122007605, + -1.5582439754376392, + -1.3742513527750697, + -1.1958249393208156, + -1.0243365165770353, + -0.8596568349141067, + -0.7045010362854772, + -0.5612043749577209, + -0.4213395870413378, + -0.29265799824540595, + -0.1747140709160461, + -0.06427383575028402, + 0.04523888440500598, + 0.1470176048847739, + 0.2335592457807787, + 0.31806725376161676, + 0.39725495085779616, + 0.4688920850625567, + 0.5378993094566618, + 0.6042473556930033, + 0.6626326850149867, + 0.7166694726914491, + 0.7682639322389797, + 0.8145747696512449, + 0.8593962881678019, + 0.902068632543713, + 0.9396647149657125, + 0.9750302682532217, + 1.0078196208749777, + 1.0370968429022625, + 1.0632475635657515, + 1.0885355014874818, + 1.1129560030203436, + 1.1368010358379101, + 1.1616439975988324, + 1.185384436834196, + 1.2084431698441171, + 1.231475823346499, + 1.254762415104005, + 1.279003680873743, + 1.304490645361735, + 1.3321634619381912, + 1.3605681174269533, + 1.3892327278655954, + 1.4186765175872023, + 1.449830915578756, + 1.481366538781976, + 1.5139104799467373, + 1.5476033220878807, + 1.583263999062211, + 1.6211933233438354, + 1.6631383793633245, + 1.7085112718607305, + 1.7566245083624843, + 1.8106609482744596, + 1.8701071709030277, + 1.9325622125965751, + 2.000600405276433, + 2.073073719730457, + 2.1487545459062662, + 2.229233110974539, + 2.314248791055779, + 2.404061883807194, + 2.503093932077756, + 2.605972973079022, + 2.713395029001061, + 2.8296484655673715, + 2.9507293024997834, + 3.073381872513424, + 3.202339794245352, + 3.337032920952903, + 3.4721955709684758, + 3.6149854181753436, + 3.7636729912526707, + 3.910178150042658, + 4.058923070848974, + 4.21475164808327, + 4.3719460968550985, + 4.529731913673303, + 4.699013206401141, + 4.872421310561653, + 5.046968437777668, + 5.229006277495889, + 5.409705435527416, + 5.593652208359495, + 5.782655045033461, + 5.973170236368014, + 6.165235802080729, + 6.362082629896817, + 6.559839729215012, + 6.758378728278356, + 6.955605431440671, + 7.152684060701755, + 7.354195314330586, + 7.556177350072287, + 7.757156526317015, + 7.961187881364418, + 8.169003680971045, + 8.377459379121646, + 8.581252383706312, + 8.781756769247409, + 8.98448890738033, + 9.185323285345376, + 9.377647316277997, + 9.574631679098811, + 9.77453789199406, + 9.968845107737579, + 10.165648840966393, + 10.369614482872768, + 10.576969644898908, + 10.796436682491988, + 11.021275163606756, + 11.246864619549918, + 11.468880277362299, + 11.693590573508914, + 11.92143585618974, + 12.146513612658033, + 12.371844212598857, + 12.598693790863255, + 12.822096235578423, + 13.035439877280268, + 13.247246640349253, + 13.460973546419812, + 13.67818491273366, + 13.889488252801932, + 14.09869730671219, + 14.310534066599072, + 14.517203507884803, + 14.719377674133092, + 14.91959255573849, + 15.11129102371526, + 15.302603365919937, + 15.497272166423574, + 15.698383337483369, + 15.900379833012433, + 16.10047374125098, + 16.301025074430772, + 16.509151973958257, + 16.746694732657037, + 16.953910521678782, + 17.177209192109522, + 17.41215293423783, + 17.64433768572708, + 17.877133390770577, + 18.119552844386924, + 18.364348477256137, + 18.615503268685643, + 18.86396729684635, + 19.119776393134917, + 19.387481743891833, + 19.65799693723779, + 19.97138913535241, + 20.252628616256146, + 20.53972116370472, + 20.864599743802877, + 21.167491705195776, + 21.479459035401458, + 21.794086367754364, + 22.11399182898066, + 22.431454120196182, + 22.750859270112684, + 23.08280638027464, + 23.40683337366142, + 23.752317076511204, + 24.111574288181092, + 24.463440793015387, + 24.83238042227069, + 25.195968103920862, + 25.539312503153802, + 25.893310860662872, + 26.24369925261043, + 26.582267823747525, + 26.934328990924996, + 27.28588487239649, + 27.621442569502996, + 27.960117425471747, + 28.303818723395178, + 28.635159948539933, + 28.96563663478973, + 29.302293916387427, + 29.63502560679554, + 29.971363294207848, + 30.29806216363941, + 30.627519894388662, + 30.96068694316108, + 31.294987541140635, + 31.623092608346326, + 31.944797115786923, + 32.27409768959656, + 32.592069090265866, + 32.89812328844543, + 33.2033668790907, + 33.50553426591979, + 33.80315270375641, + 34.10199116335442, + 34.40374116129653, + 34.70037769128913, + 34.99930901872234, + 35.3027502455714, + 35.61019483415942, + 35.91778870110349, + 36.23177032401961, + 36.549318572671865, + 36.86447146895361, + 37.18297074853924, + 37.50051483488108, + 37.812978772462074, + 38.12853715955711, + 38.44735728290711, + 38.761536901895255, + 39.07845222453753, + 39.398809727184464, + 39.71473270566849, + 40.029760352806896, + 40.34504792610765, + 40.656417875372846, + 40.968938220175374, + 41.28462245556623, + 41.595517406598056, + 41.9094211795519, + 42.22432579850013, + 42.533473369050185, + 42.8382291810717, + 43.149608904374205, + 43.451222239978314, + 43.7461698096567, + 44.055228739426425, + 44.3520907788295, + 44.64503373770526, + 44.950728556177175, + 45.24935317712527, + 45.54098436312996, + 45.842943558877685, + 46.14755303458895, + 46.443326146505704, + 46.73854919902926, + 47.04786138192352, + 47.35185725399669, + 47.6496011629088, + 47.95643597017896, + 48.26021279372815, + 48.5526815219461, + 48.85167784575071, + 49.15353998314599, + 49.44431352175967, + 49.73911939311548, + 50.03882848645932, + 50.32720195041783, + 50.61033855379421, + 50.89366193587728, + 51.167884941565404, + 51.442476792158224, + 51.70740478669174, + 51.97455183959459, + 52.2424372398762, + 52.50955159749591, + 52.7824441181921, + 53.051639281404725, + 53.324425481435284, + 53.60486635010587, + 53.88963555239475, + 54.17262292819129, + 54.4593666694105, + 54.746556348026466, + 55.0305015067513, + 55.31720677899201, + 55.59453917403792, + 55.86299504245453, + 56.15888068132426, + 56.43126106967637, + 56.689559026047526, + 56.94065230347727, + 57.20232586978722, + 57.46099179998578, + 57.716605912095446, + 57.9667704481623, + 58.22562514258538, + 58.48279049995408, + 58.73193463906139, + 58.97679471021057, + 59.231726255130766, + 59.48618960316225, + 59.72657914780032, + 59.97475629090222, + 60.23213442959812, + 60.47564590161082, + 60.7049426999666, + 60.93445010429704, + 61.16012701900658, + 61.3628146677447, + 61.550008156074576, + 61.74411336584359, + 61.93120147403796, + 62.101802479904414, + 62.270331330755454, + 62.43413388237453, + 62.58517546562002, + 62.73057604318588, + 62.8658714391403, + 62.989543574071305, + 63.103099285653244, + 63.20404152139367, + 63.28437587397921, + 63.34853626378916, + 63.39811140601858, + 63.43054360930116, + 63.45097372267109, + 63.4567870942156, + 63.4447036226012, + 63.415166511477636, + 63.37023820702246, + 63.31125332955198, + 63.23868736126066, + 63.14754852230307, + 63.03907964589805, + 62.92303318853251, + 62.78694141392481, + 62.63013965343733, + 62.47033102323899, + 62.291344581506586, + 62.09359031103882, + 61.89338210735949, + 61.68071930303039, + 61.44532632829423, + 61.203398502780615, + 60.956911902839714, + 60.69511980115445, + 60.42411300080536, + 60.151264974959936, + 59.874515231224194, + 59.597389428236895, + 59.31315620970049, + 59.021405078511236, + 58.73348260878832, + 58.44388700203228, + 58.14274849214298, + 57.845826988627806, + 57.547992367501905, + 57.23870777395376, + 56.928117561617285, + 56.62382853069168, + 56.31482853049869, + 55.990849826911465, + 55.683937631249485, + 55.379659002614254, + 55.05940881942585, + 54.7478383274686, + 54.446228891852904, + 54.12854095685096, + 53.81675817839018, + 53.51092358754183, + 53.191921350580614, + 52.874242507329065, + 52.560490915928355, + 52.23665547370583, + 51.9077213338404, + 51.586335427714964, + 51.25869299864215, + 50.92217315579611, + 50.58992709820675, + 50.25454632426013, + 49.91197629794347, + 49.57380078223236, + 49.237340611949016, + 48.898109266487566, + 48.561150029770815, + 48.217675361077035, + 47.8814988613294, + 47.538440549060454, + 47.193064510530945, + 46.84970730673195, + 46.50781183219243, + 46.16979702769043, + 45.82343644077698, + 45.479765835147404, + 45.13888378364644, + 44.79218792981562, + 44.44490713561993, + 44.0988326524609, + 43.75639413298596, + 43.405006335112816, + 43.055527911170955, + 42.7086224383385, + 42.35636752516571, + 42.01094060050921, + 41.66224869226204, + 41.30453120325186, + 40.955322427633995, + 40.606122509777826, + 40.23671831132266, + 39.87349021139226, + 39.5275216361299, + 39.15532054602195, + 38.7817917199122, + 38.42861344698394, + 38.061284715268485, + 37.68275186248695, + 37.327194044766365, + 36.97288601560147, + 36.59687620499849, + 36.23856852027886, + 35.88109543832795, + 35.51698266869542, + 35.151454114699064, + 34.794316267355306, + 34.437446570826715, + 34.0771206780121, + 33.71134847630277, + 33.35259399364076, + 33.003896861735065, + 32.640723827922415, + 32.28507059277639, + 31.936035330965904, + 31.586702433075484, + 31.22925195912165, + 30.88262143045321, + 30.53913232170126, + 30.188714527696792, + 29.840928928588454, + 29.48492688340245, + 29.125366374489975, + 28.76159707627873, + 28.402096387015956, + 28.04029167738964, + 27.677618347007837, + 27.324717705275482, + 26.96516969116016, + 26.60742238152953, + 26.25813582849179, + 25.940644512843257, + 25.64412781515667, + 25.348022830332283, + 25.054248705830318, + 24.762940172729472, + 24.474128236011488, + 24.183142438830497, + 23.889583459808744, + 23.60377062481296, + 23.31867446763228, + 23.00573403717028, + 22.69543862677699, + 22.38831817031561, + 22.07397291674327, + 21.77221620303394, + 21.474625655377526, + 21.159855111507262, + 20.860472983457548, + 20.57213799635636, + 20.259065357779058, + 19.974570675468115, + 19.717712944410955, + 19.440432089622206, + 19.16813345630301, + 18.908419499224287, + 18.64000437675221, + 18.36595897269595, + 18.101705674415747, + 17.836092414791384, + 17.56766658637108, + 17.29630341598981, + 17.025696047063235, + 16.75357804943685, + 16.481298915640618, + 16.21239540766006, + 15.937971120749795, + 15.666811668372599, + 15.391689270983614, + 15.117745913680936, + 14.848566220763917, + 14.540019487081059, + 14.222796660348196, + 13.903347266645358, + 13.579127085268404, + 13.257103352453145, + 12.937332081227593, + 12.61581250883567, + 12.287693670984922, + 11.954463566316953, + 11.631394681600423, + 11.329342568479117, + 11.031985211164129, + 10.737523628601888, + 10.45177322208383, + 10.159690898535434, + 9.86528611566276, + 9.585463943766001, + 9.302510829422065, + 9.018771681351822, + 8.740721821328417, + 8.451412847625747, + 8.16055976991773, + 7.940739885595652, + 7.649689547274853, + 7.355041201225335, + 7.073086663607159, + 6.781403498982336, + 6.480975198189406, + 6.193000995912802, + 5.894118683471748, + 5.604056865569893, + 5.33068637775697, + 5.0487330235721695, + 4.759880363605886, + 4.478298131655137, + 4.196421392600951, + 3.905844195969257, + 3.622675096119216, + 3.3335103833622286, + 3.0471742784361973, + 2.7719845417630884, + 2.487867692040898, + 2.2085807352402806, + 1.9386381623244573, + 1.6696997892504515, + 1.4008923722223003, + 1.1325338854488094, + 0.8698931640746329, + 0.6058550815292704, + 0.34178591209782944, + 0.07667844538865048, + -0.19026593451683016, + -0.4554406244379945, + -0.7270459916318173, + -0.9993729438317401, + -1.2657127999327225, + -1.5359056209694093, + -1.80910351197689, + -2.0707859508561195, + -2.3171929634210486, + -2.57303096028214, + -2.8298091729269363, + -3.06656586192069, + -3.2899355731611752, + -3.5058629323482395, + -3.7002218758350507, + -3.8799659060810683, + -4.044154401469258, + -4.188005818183804, + -4.3152184885170035, + -4.431354366367503, + -4.514703010052084, + -4.582047960812328, + -4.631528872229713, + -4.660910020821575, + -4.6708403048030105, + -4.662449926632847, + -4.637595003710751, + -4.596236011176684, + -4.537947385026952, + -4.376888542703274, + -4.288209726394857, + -4.18429492589649, + -4.066784639944364, + -3.9387904539825316, + -3.799571967547997, + -3.653433657150335, + -3.501583167393676, + -3.3401169800524984, + -3.1744481619690124, + -3.0052437357619475, + -2.829155463434466, + -2.6527959456576737, + -2.475772365932245, + -2.2955140655935677, + -2.1163822744969565, + -1.9392996466493302, + -1.7651734640595305, + -1.5942498473022266, + -1.4247547430679854, + -1.2604125493983682, + -1.1006254540448448, + -0.9428929999493842, + -0.7885739354253732, + -0.6394382491121076, + -0.49422949338813715, + -0.3547025436789347, + -0.2220377492865131, + -0.09497282596831103, + 0.021735167796699595, + 0.13285858280024343, + 0.24087676995385654, + 0.33923224114094186, + 0.4329200110885978, + 0.5201430763632404, + 0.6076261201085276, + 0.6879757431943925, + 0.7578119823986187, + 0.8269555418893362, + 0.8913029716133205, + 0.9503621390414663, + 1.0076633251910367, + 1.0619069384761641, + 1.1108600056309395, + 1.154451832954652, + 1.1949563456065757, + 1.2306989835976558, + 1.2636346948814599, + 1.2938187560504752, + 1.3188102651418132, + 1.3400979295708186, + 1.3587029727938362, + 1.3736961944225057, + 1.3874170183239944, + 1.4002813212691323, + 1.413864282540611, + 1.428639923213225, + 1.4439159929580396, + 1.4588185530447295, + 1.4716824922168787, + 1.4835117408193126, + 1.4944732530514964, + 1.505037309620955, + 1.5159365970329328, + 1.5279182777271618, + 1.5710227590004497, + 1.585487163567568, + 1.601561593277515, + 1.6191296376214352, + 1.6392118118171357, + 1.661332817227168, + 1.683818426776412, + 1.7097134479140736, + 1.7383053758480917, + 1.7712132265080818, + 1.8082864177801734, + 1.8495958259515872, + 1.8971048618132835, + 1.9495414443591164, + 2.0089977824310385, + 2.0774488692880864, + 2.152719910738682, + 2.238964543324992, + 2.334237456808289, + 2.4368837415056133, + 2.548951046986737, + 2.6712010864801186, + 2.804199387424994, + 2.9489350548037434, + 3.104840352919012, + 3.2696446307951232, + 3.449537370291477, + 3.6422762528119206, + 3.844964818735719, + 4.064028170637739, + 4.296194383137763, + 4.536128453129303, + 4.780515740575605, + 5.068595331826337, + 5.339755000188887, + 5.614200312412775, + 5.903984607704475, + 6.204051350614986, + 6.502448630503951, + 6.810922050815234, + 7.129056537553518, + 7.449288520433644, + 7.773967266604292, + 8.107895570113833, + 8.446125235123965, + 8.788662654274459, + 9.13733022137939, + 9.492647484169803, + 9.849955649461563, + 10.21254506524584, + 10.577595066343726, + 10.945159109465557, + 11.317011962849532, + 11.695576288011447, + 12.073581835360525, + 12.45651816101892, + 12.83768461737093, + 13.225855182239112, + 13.6045467550301, + 13.982960369263015, + 14.37046998740073, + 14.753653591504062, + 15.128301092639678, + 15.48333670659692, + 15.866854354862518, + 16.245350633101292, + 16.624730674923363, + 17.001686838803085, + 17.37919471421535, + 17.740406988155204, + 18.101336461510847, + 18.468215679311406, + 18.82581382576176, + 19.18161067675971, + 19.535407199454607, + 19.884345087594586, + 20.228653998324123, + 20.548386356183464, + 20.865869997294162, + 21.16885830062872, + 21.444246808429096, + 21.723230991549734, + 21.981530352651177, + 22.224328239948395, + 22.46410538987838, + 22.682047779183893, + 22.87564032840609, + 23.07241333225752, + 23.249333466160344, + 23.402534058412485, + 23.554669446076673, + 23.689115788959498, + 23.803609375044548, + 23.914176643879845, + 24.00819998804275, + 24.090429572476623, + 24.15856535870222, + 24.21198547003588, + 24.258824715173773, + 24.299506484657044, + 24.333927203988285, + 24.36638965023104, + 24.3987771413772, + 24.4317546647794, + 24.463807479798007, + 24.47374998174773, + 24.482187505217095, + 24.489667268472083, + 24.497212999751582, + 24.50380722017657, + 24.50955393584218, + 24.515152386071996, + 24.520318624688684, + 24.5248045628062, + 24.52850286369933, + 24.562741464181546, + 24.600868556033113, + 24.639572183517366, + 24.678999202340567, + 24.71910268613251, + 24.752167541806834, + 24.79364139014063, + 24.835708363767267, + 24.87896621837055, + 24.9232279506598, + 24.934048566691608, + 24.94107921159258, + 24.948355187402903, + 24.955586820345676, + 24.962806447321853, + 24.97021256670934, + 24.977547770122076, + 24.98494706269543, + 24.99242952739627, + 24.99994437339902, + 25.02540271299319, + 25.052916655074828, + 25.08056116530959, + 25.108168293242098, + 25.135808603887792, + 25.163485883998344, + 25.19115746111873, + 25.218876122759994, + 25.24661949756898, + 25.274470489049747, + 25.284313636520654, + 25.292359845297213, + 25.30049146549067, + 25.308518479646892, + 25.316455103602966, + 25.32458102322264, + 25.332738211605577, + 25.340816196491332, + 25.349086424508247, + 25.357295176651835, + 25.403074324577638, + 25.45374928722011, + 25.504408168173335, + 25.55487276861665, + 25.605032335771533, + 25.655136174273608, + 25.705378138595986, + 25.755388975120873, + 25.80516652331133, + 25.855482764168823, + 25.866115316851094, + 25.872209004406198, + 25.87815187371181, + 25.88382182578971, + 25.889485629453922, + 25.895124265033186, + 25.900828708697823, + 25.907149898069797, + 25.912957769240567, + 25.91864769467925, + 25.920183502332854, + 25.921636104878875, + 25.922860010393627, + 25.92409529747196, + 25.925571771507055, + 25.92699075098118, + 25.928285237895963, + 25.929639765365053 + ], + "yaxis": "y" + }, + { + "mode": "markers", + "showlegend": false, + "type": "scatter", + "x": [ + -35.97329684506414, + -75.10034519689322, + -1.038764248227812, + -36.0892694400111 + ], + "y": [ + 26.316580859038513, + 21.011440906439965, + -12.138119313965529, + 72.35004639855165 + ] + } + ], + "layout": { + "legend": { + "tracegroupgap": 0 + }, + "margin": { + "b": 0, + "l": 0, + "r": 0, + "t": 0 + }, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "heatmapgl": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmapgl" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "xaxis": { + "anchor": "y", + "domain": [ + 0, + 1 + ], + "title": { + "text": "x" + } + }, + "yaxis": { + "anchor": "x", + "domain": [ + 0, + 1 + ], + "scaleanchor": "x", + "scaleratio": 1, + "title": { + "text": "y" + } + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "fig = px.scatter(x=positions[:,0],y=positions[:,1])\n", + "fig.add_scatter(x=landmarks[:,0], y=landmarks[:,1], mode=\"markers\", showlegend= False)\n", + "fig.update_layout(margin=dict(l=0, r=0, t=0, b=0))\n", + "fig.update_yaxes(scaleanchor = \"x\", scaleratio = 1)\n", + "fig.show()" + ] + }, + { + "cell_type": "code", + "execution_count": 60, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([[-35.97329685, 26.31658086],\n", + " [-75.1003452 , 21.01144091],\n", + " [ -1.03876425, -12.13811931],\n", + " [-36.08926944, 72.3500464 ]])" + ] + }, + "execution_count": 60, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "landmarks" + ] + } + ], + "metadata": { + "interpreter": { + "hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101" + }, + "kernelspec": { + "display_name": "Python 3.8.12 ('nbdev')", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.12" + }, + "orig_nbformat": 4 + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/python/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py index a29b0f263..09ac4c564 100644 --- a/python/gtsam/tests/test_GaussianFactorGraph.py +++ b/python/gtsam/tests/test_GaussianFactorGraph.py @@ -23,23 +23,23 @@ from gtsam.utils.test_case import GtsamTestCase def create_graph(): """Create a basic linear factor graph for testing""" graph = gtsam.GaussianFactorGraph() - + x0 = X(0) x1 = X(1) x2 = X(2) - + BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE) - graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE) + graph.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE) graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE) return graph, (x0, x1, x2) + class TestGaussianFactorGraph(GtsamTestCase): """Tests for Gaussian Factor Graphs.""" - def test_fg(self): """Test solving a linear factor graph""" graph, X = create_graph() @@ -71,7 +71,7 @@ class TestGaussianFactorGraph(GtsamTestCase): self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) - + def test_linearMarginalization(self): """Marginalize a linear factor graph""" graph, X = create_graph() @@ -88,5 +88,23 @@ class TestGaussianFactorGraph(GtsamTestCase): self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) + def test_ordering(self): + """Test ordering""" + gfg, keys = create_graph() + ordering = gtsam.Ordering() + for key in keys[::-1]: + ordering.push_back(key) + + bn = gfg.eliminateSequential(ordering) + self.assertEqual(bn.size(), 3) + + keyVector = gtsam.KeyVector() + keyVector.append(keys[2]) + #TODO(Varun) Below code causes segfault in Debug config + # ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector) + # bn = gfg.eliminateSequential(ordering) + # self.assertEqual(bn.size(), 3) + + if __name__ == '__main__': unittest.main() diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index e5ffbad7d..860487db2 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -8,11 +8,11 @@ See LICENSE for the license information Pose2 unit tests. Author: Frank Dellaert & Duy Nguyen Ta & John Lambert """ +import math import unittest -import numpy as np - import gtsam +import numpy as np from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -26,6 +26,34 @@ class TestPose2(GtsamTestCase): actual = Pose2.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) + def test_transformTo(self): + """Test transformTo method.""" + pose = Pose2(2, 4, -math.pi/2) + actual = pose.transformTo(Point2(3, 2)) + expected = Point2(2, 1) + self.gtsamAssertEquals(actual, expected, 1e-6) + + # multi-point version + points = np.stack([Point2(3, 2), Point2(3, 2)]).T + actual_array = pose.transformTo(points) + self.assertEqual(actual_array.shape, (2, 2)) + expected_array = np.stack([expected, expected]).T + np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) + + def test_transformFrom(self): + """Test transformFrom method.""" + pose = Pose2(2, 4, -math.pi/2) + actual = pose.transformFrom(Point2(2, 1)) + expected = Point2(3, 2) + self.gtsamAssertEquals(actual, expected, 1e-6) + + # multi-point version + points = np.stack([Point2(2, 1), Point2(2, 1)]).T + actual_array = pose.transformFrom(points) + self.assertEqual(actual_array.shape, (2, 2)) + expected_array = np.stack([expected, expected]).T + np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) + def test_align(self) -> None: """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index 411828890..cde71de53 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -15,7 +15,7 @@ import unittest import numpy as np import gtsam -from gtsam import Point3, Pose3, Rot3 +from gtsam import Point3, Pose3, Rot3, Point3Pairs from gtsam.utils.test_case import GtsamTestCase @@ -30,13 +30,34 @@ class TestPose3(GtsamTestCase): actual = T2.between(T3) self.gtsamAssertEquals(actual, expected, 1e-6) - def test_transform_to(self): + def test_transformTo(self): """Test transformTo method.""" - transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) - actual = transform.transformTo(Point3(3, 2, 10)) + pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) + actual = pose.transformTo(Point3(3, 2, 10)) expected = Point3(2, 1, 10) self.gtsamAssertEquals(actual, expected, 1e-6) + # multi-point version + points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T + actual_array = pose.transformTo(points) + self.assertEqual(actual_array.shape, (3, 2)) + expected_array = np.stack([expected, expected]).T + np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) + + def test_transformFrom(self): + """Test transformFrom method.""" + pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) + actual = pose.transformFrom(Point3(2, 1, 10)) + expected = Point3(3, 2, 10) + self.gtsamAssertEquals(actual, expected, 1e-6) + + # multi-point version + points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T + actual_array = pose.transformFrom(points) + self.assertEqual(actual_array.shape, (3, 2)) + expected_array = np.stack([expected, expected]).T + np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) + def test_range(self): """Test range method.""" l1 = Point3(1, 0, 0) @@ -81,6 +102,24 @@ class TestPose3(GtsamTestCase): actual.deserialize(serialized) self.gtsamAssertEquals(expected, actual, 1e-10) + def test_align_squares(self): + """Test if Align method can align 2 squares.""" + square = np.array([[0,0,0],[0,1,0],[1,1,0],[1,0,0]], float).T + sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0)) + transformed = sTt.transformTo(square) + + st_pairs = Point3Pairs() + for j in range(4): + st_pairs.append((square[:,j], transformed[:,j])) + + # Recover the transformation sTt + estimated_sTt = Pose3.Align(st_pairs) + self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10) + + # Matrix version + estimated_sTt = Pose3.Align(square, transformed) + self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index 6eb05eeee..ebc77e2e3 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -10,9 +10,6 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python) """ import unittest -import numpy as np - -import gtsam import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam from gtsam import symbol @@ -20,8 +17,9 @@ from gtsam.utils.test_case import GtsamTestCase class TestVisualISAMExample(GtsamTestCase): - + """Test class for ISAM2 with visual landmarks.""" def test_VisualISAMExample(self): + """Test to see if ISAM works as expected for a simple visual SLAM example.""" # Data Options options = generator.Options() options.triangle = False @@ -39,19 +37,22 @@ class TestVisualISAMExample(GtsamTestCase): data, truth = generator.generate_data(options) # Initialize iSAM with the first pose and points - isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) + isam, result, nextPose = visual_isam.initialize( + data, truth, isamOptions) # Main loop for iSAM: stepping through all poses for currentPose in range(nextPose, options.nrCameras): - isam, result = visual_isam.step(data, isam, result, truth, currentPose) + isam, result = visual_isam.step(data, isam, result, truth, + currentPose) - for i in range(len(truth.cameras)): + for i, _ in enumerate(truth.cameras): pose_i = result.atPose3(symbol('x', i)) self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) - for j in range(len(truth.points)): + for j, _ in enumerate(truth.points): point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_sam.py b/python/gtsam/tests/test_sam.py new file mode 100644 index 000000000..e01b9c1d1 --- /dev/null +++ b/python/gtsam/tests/test_sam.py @@ -0,0 +1,55 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Basis unit tests. +Author: Frank Dellaert & Varun Agrawal (Python) +""" +import unittest + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestSam(GtsamTestCase): + """ + Tests python binding for classes/functions in `sam.i`. + """ + def test_RangeFactor2D(self): + """ + Test that `measured` works as expected for RangeFactor2D. + """ + measurement = 10.0 + factor = gtsam.RangeFactor2D(1, 2, measurement, + gtsam.noiseModel.Isotropic.Sigma(1, 1)) + self.assertEqual(measurement, factor.measured()) + + def test_BearingFactor2D(self): + """ + Test that `measured` works as expected for BearingFactor2D. + """ + measurement = gtsam.Rot2(.3) + factor = gtsam.BearingFactor2D(1, 2, measurement, + gtsam.noiseModel.Isotropic.Sigma(1, 1)) + self.gtsamAssertEquals(measurement, factor.measured()) + + def test_BearingRangeFactor2D(self): + """ + Test that `measured` works as expected for BearingRangeFactor2D. + """ + range_measurement = 10.0 + bearing_measurement = gtsam.Rot2(0.3) + factor = gtsam.BearingRangeFactor2D( + 1, 2, bearing_measurement, range_measurement, + gtsam.noiseModel.Isotropic.Sigma(2, 1)) + measurement = factor.measured() + + self.assertEqual(range_measurement, measurement.range()) + self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 068b39eca..5eaad45dc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -3,14 +3,14 @@ set (tests_exclude "") if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richard & Jason & Frank - # clang linker segfaults on large testSerializationSLAM - list (APPEND tests_exclude "testSerializationSLAM.cpp") + # clang linker segfaults on large testSerializationSlam + list (APPEND tests_exclude "testSerializationSlam.cpp") endif() # Build tests gtsamAddTestsGlob(tests "test*.cpp" "${tests_exclude}" "gtsam") if(MSVC) - set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp" + set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSlam.cpp" APPEND PROPERTY COMPILE_FLAGS "/bigobj") endif() diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 75425a0cd..6d23144aa 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -231,7 +231,7 @@ TEST(ExpressionFactor, Shallow) { Pose3_ x_(1); Point3_ p_(2); - // Construct expression, concise evrsion + // Construct expression, concise version Point2_ expression = project(transformTo(x_, p_)); // Get and check keys and dims diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index a3d1e4e9b..c3335ce20 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -98,6 +98,30 @@ TEST(GncOptimizer, gncConstructor) { CHECK(gnc.equals(gnc2)); } +/* ************************************************************************* */ +TEST(GncOptimizer, solverParameterParsing) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor + // on a 2D point + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + LevenbergMarquardtParams lmParams; + lmParams.setMaxIterations(0); // forces not to perform optimization + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + Values result = gnc.optimize(); + + // check that LM did not perform optimization and result is the same as the initial guess + DOUBLES_EQUAL(fg.error(initial), fg.error(result), tol); + + // also check the params: + DOUBLES_EQUAL(0.0, gncParams.baseOptimizerParams.maxIterations, tol); +} + /* ************************************************************************* */ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { auto fg = example::sharedNonRobustFactorGraphWithOutliers(); diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSlam.cpp similarity index 90% rename from tests/testSerializationSLAM.cpp rename to tests/testSerializationSlam.cpp index 496122419..ea7038635 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSlam.cpp @@ -19,16 +19,16 @@ #include #include + +#include #include + #include #include -#include #include -#include +#include #include -#include -#include -#include + #include #include #include @@ -44,8 +44,16 @@ #include #include +#include +#include +#include +#include +#include #include +#include +#include + using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; @@ -592,6 +600,78 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(genericStereoFactor3D)); } +/* ************************************************************************* */ +// Read from XML file +namespace { +static GaussianFactorGraph read(const string& name) { + auto inputFile = findExampleDataFile(name); + ifstream is(inputFile); + if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile); + boost::archive::xml_iarchive in_archive(is); + GaussianFactorGraph Ab; + in_archive >> boost::serialization::make_nvp("graph", Ab); + return Ab; +} +} // namespace + +/* ************************************************************************* */ +// Read from XML file +TEST(SubgraphSolver, Solves) { + using gtsam::example::planarGraph; + + // Create preconditioner + SubgraphPreconditioner system; + + // We test on three different graphs + const auto Ab1 = planarGraph(3).first; + const auto Ab2 = read("toy3D"); + const auto Ab3 = read("randomGrid3D"); + + // For all graphs, test solve and solveTranspose + for (const auto& Ab : {Ab1, Ab2, Ab3}) { + // Call build, a non-const method needed to make solve work :-( + KeyInfo keyInfo(Ab); + std::map lambda; + system.build(Ab, keyInfo, lambda); + + // Create a perturbed (non-zero) RHS + const auto xbar = system.Rc1().optimize(); // merely for use in zero below + auto values_y = VectorValues::Zero(xbar); + auto it = values_y.begin(); + it->second.setConstant(100); + ++it; + it->second.setConstant(-100); + + // Solve the VectorValues way + auto values_x = system.Rc1().backSubstitute(values_y); + + // Solve the matrix way, this really just checks BN::backSubstitute + // This only works with Rc1 ordering, not with keyInfo ! + // TODO(frank): why does this not work with an arbitrary ordering? + const auto ord = system.Rc1().ordering(); + const Matrix R1 = system.Rc1().matrix(ord).first; + auto ord_y = values_y.vector(ord); + auto vector_x = R1.inverse() * ord_y; + EXPECT(assert_equal(vector_x, values_x.vector(ord))); + + // Test that 'solve' does implement x = R^{-1} y + // We do this by asserting it gives same answer as backSubstitute + // Only works with keyInfo ordering: + const auto ordering = keyInfo.ordering(); + auto vector_y = values_y.vector(ordering); + const size_t N = R1.cols(); + Vector solve_x = Vector::Zero(N); + system.solve(vector_y, solve_x); + EXPECT(assert_equal(values_x.vector(ordering), solve_x)); + + // Test that transposeSolve does implement x = R^{-T} y + // We do this by asserting it gives same answer as backSubstituteTranspose + auto values_x2 = system.Rc1().backSubstituteTranspose(values_y); + Vector solveT_x = Vector::Zero(N); + system.transposeSolve(vector_y, solveT_x); + EXPECT(assert_equal(values_x2.vector(ordering), solveT_x)); + } +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index eeba38b68..c5b4e42ec 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -29,10 +29,8 @@ #include -#include #include #include -#include #include using namespace boost::assign; @@ -197,75 +195,6 @@ TEST(SubgraphPreconditioner, system) { EXPECT(assert_equal(expected_g, vec(g))); } -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor") - -// Read from XML file -static GaussianFactorGraph read(const string& name) { - auto inputFile = findExampleDataFile(name); - ifstream is(inputFile); - if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile); - boost::archive::xml_iarchive in_archive(is); - GaussianFactorGraph Ab; - in_archive >> boost::serialization::make_nvp("graph", Ab); - return Ab; -} - -TEST(SubgraphSolver, Solves) { - // Create preconditioner - SubgraphPreconditioner system; - - // We test on three different graphs - const auto Ab1 = planarGraph(3).first; - const auto Ab2 = read("toy3D"); - const auto Ab3 = read("randomGrid3D"); - - // For all graphs, test solve and solveTranspose - for (const auto& Ab : {Ab1, Ab2, Ab3}) { - // Call build, a non-const method needed to make solve work :-( - KeyInfo keyInfo(Ab); - std::map lambda; - system.build(Ab, keyInfo, lambda); - - // Create a perturbed (non-zero) RHS - const auto xbar = system.Rc1().optimize(); // merely for use in zero below - auto values_y = VectorValues::Zero(xbar); - auto it = values_y.begin(); - it->second.setConstant(100); - ++it; - it->second.setConstant(-100); - - // Solve the VectorValues way - auto values_x = system.Rc1().backSubstitute(values_y); - - // Solve the matrix way, this really just checks BN::backSubstitute - // This only works with Rc1 ordering, not with keyInfo ! - // TODO(frank): why does this not work with an arbitrary ordering? - const auto ord = system.Rc1().ordering(); - const Matrix R1 = system.Rc1().matrix(ord).first; - auto ord_y = values_y.vector(ord); - auto vector_x = R1.inverse() * ord_y; - EXPECT(assert_equal(vector_x, values_x.vector(ord))); - - // Test that 'solve' does implement x = R^{-1} y - // We do this by asserting it gives same answer as backSubstitute - // Only works with keyInfo ordering: - const auto ordering = keyInfo.ordering(); - auto vector_y = values_y.vector(ordering); - const size_t N = R1.cols(); - Vector solve_x = Vector::Zero(N); - system.solve(vector_y, solve_x); - EXPECT(assert_equal(values_x.vector(ordering), solve_x)); - - // Test that transposeSolve does implement x = R^{-T} y - // We do this by asserting it gives same answer as backSubstituteTranspose - auto values_x2 = system.Rc1().backSubstituteTranspose(values_y); - Vector solveT_x = Vector::Zero(N); - system.transposeSolve(vector_y, solveT_x); - EXPECT(assert_equal(values_x2.vector(ordering), solveT_x)); - } -} - /* ************************************************************************* */ TEST(SubgraphPreconditioner, conjugateGradients) { // Build a planar graph diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 4fe643047..833f11355 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -115,8 +115,8 @@ TEST(TranslationRecovery, TwoPoseTest) { const auto result = algorithm.run(/*scale=*/3.0); // Check result for first two translations, determined by prior - EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); } TEST(TranslationRecovery, ThreePoseTest) { @@ -152,9 +152,9 @@ TEST(TranslationRecovery, ThreePoseTest) { const auto result = algorithm.run(/*scale=*/3.0); // Check result - EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(3))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(3), 1e-8)); } TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { @@ -189,9 +189,9 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { const auto result = algorithm.run(/*scale=*/3.0); // Check result - EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(3, 0, 0), result.at(2))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(2), 1e-8)); } TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { @@ -230,10 +230,10 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { const auto result = algorithm.run(/*scale=*/4.0); // Check result - EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(4, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(4, 0, 0), result.at(2))); - EXPECT(assert_equal(Point3(2, -2, 0), result.at(3))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(2), 1e-8)); + EXPECT(assert_equal(Point3(2, -2, 0), result.at(3), 1e-8)); } TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { @@ -260,9 +260,9 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { const auto result = algorithm.run(/*scale=*/4.0); // Check result - EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(0, 0, 0), result.at(1))); - EXPECT(assert_equal(Point3(0, 0, 0), result.at(2))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); } /* ************************************************************************* */ diff --git a/timing/timeBatch.cpp b/timing/timeBatch.cpp index 4ed1a4555..f59039fa7 100644 --- a/timing/timeBatch.cpp +++ b/timing/timeBatch.cpp @@ -28,7 +28,7 @@ int main(int argc, char *argv[]) { cout << "Loading data..." << endl; - string datasetFile = findExampleDataFile("w10000-odom"); + string datasetFile = findExampleDataFile("w10000"); std::pair data = load2D(datasetFile); diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 6e0f4ccdf..5e3fc9189 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -72,7 +72,7 @@ int main(int argc, char *argv[]) { cout << "Loading data..." << endl; gttic_(Find_datafile); - //string datasetFile = findExampleDataFile("w10000-odom"); + //string datasetFile = findExampleDataFile("w10000"); string datasetFile = findExampleDataFile("victoria_park"); std::pair data = load2D(datasetFile); diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 79d7432c8..7af798887 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -16,7 +16,10 @@ * @date July 5, 2015 */ +#pragma once + #include +#include #include #include #include diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py index 1a3f10bf5..31d8d4444 100755 --- a/wrap/gtwrap/pybind_wrapper.py +++ b/wrap/gtwrap/pybind_wrapper.py @@ -412,7 +412,7 @@ class PybindWrapper: def wrap_instantiated_declaration( self, instantiated_decl: instantiator.InstantiatedDeclaration): - """Wrap the class.""" + """Wrap the forward declaration.""" module_var = self._gen_module_var(instantiated_decl.namespaces()) cpp_class = instantiated_decl.to_cpp() if cpp_class in self.ignore_classes: @@ -420,7 +420,7 @@ class PybindWrapper: res = ( '\n py::class_<{cpp_class}, ' - '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")' + '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}");' ).format(shared_ptr_type=('boost' if self.use_boost else 'std'), cpp_class=cpp_class, class_name=instantiated_decl.name, diff --git a/wrap/matlab.h b/wrap/matlab.h index fbed0b2e2..645ba8edf 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -37,15 +37,16 @@ extern "C" { #include } -#include +#include #include +#include #include -#include -#include -#include #include +#include #include +#include +#include using namespace std; using namespace boost; // not usual, but for conciseness of generated code diff --git a/wrap/pybind11/tests/object.h b/wrap/pybind11/tests/object.h index 9235f19c2..9fbbc69f0 100644 --- a/wrap/pybind11/tests/object.h +++ b/wrap/pybind11/tests/object.h @@ -1,5 +1,4 @@ -#if !defined(__OBJECT_H) -#define __OBJECT_H +#pragma once #include #include "constructor_stats.h" @@ -171,5 +170,3 @@ public: private: T *m_ptr; }; - -#endif /* __OBJECT_H */ diff --git a/wrap/tests/expected/python/class_pybind.cpp b/wrap/tests/expected/python/class_pybind.cpp index fd5398912..95171def4 100644 --- a/wrap/tests/expected/python/class_pybind.cpp +++ b/wrap/tests/expected/python/class_pybind.cpp @@ -105,7 +105,7 @@ PYBIND11_MODULE(class_py, m_) { return redirect.str(); }, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter); - py::class_, std::shared_ptr>>(m_, "SuperCoolFactorPose3") + py::class_, std::shared_ptr>>(m_, "SuperCoolFactorPose3"); #include "python/specializations.h" diff --git a/wrap/tests/fixtures/inheritance.i b/wrap/tests/fixtures/inheritance.i index e63f8e689..a3b64ed4b 100644 --- a/wrap/tests/fixtures/inheritance.i +++ b/wrap/tests/fixtures/inheritance.i @@ -1,6 +1,5 @@ // A base class virtual class MyBase { - }; // A templated class