diff --git a/.gitignore b/.gitignore index 04e8e76d1..b3ea282dd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build* +.idea *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c4229ed5..77434d135 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,8 +66,9 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) -option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF) +option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) +option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -90,8 +91,8 @@ if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4) message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.") endif() -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.") +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.") endif() # Flags for choosing default packaging tools @@ -115,11 +116,11 @@ if(MSVC) endif() endif() -find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) +find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono) # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR - NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY OR NOT Boost_REGEX_LIBRARY) + NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") endif() @@ -127,7 +128,7 @@ option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} - ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) + ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY}) if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) @@ -305,7 +306,7 @@ endif() # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. include_directories(BEFORE - gtsam/3rdparty/UFconfig + gtsam/3rdparty/SuiteSparse_config gtsam/3rdparty/CCOLAMD/Include ${METIS_INCLUDE_DIRECTORIES} ${PROJECT_SOURCE_DIR} @@ -485,13 +486,14 @@ message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") message(STATUS " CPack Generator : ${CPACK_GENERATOR}") message(STATUS "GTSAM flags ") -print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") -print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") -print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") -print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") -print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") -print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") -print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") +print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") +print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") +print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") +print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ") +print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") +print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index ff1f1b692..23d1e2573 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -68,6 +68,12 @@ protected: testGroup##testName##Instance; \ void testGroup##testName##Test::run (TestResult& result_) +/** + * Declare friend in a class to test its private methods + */ +#define FRIEND_TEST(testGroup, testName) \ + friend class testGroup##testName##Test; + /** * For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this * will not wrap execution with a try/catch block diff --git a/CppUnitLite/TestResult.h b/CppUnitLite/TestResult.h index a30275647..b64b40d75 100644 --- a/CppUnitLite/TestResult.h +++ b/CppUnitLite/TestResult.h @@ -18,8 +18,6 @@ // /////////////////////////////////////////////////////////////////////////////// - - #ifndef TESTRESULT_H #define TESTRESULT_H diff --git a/LICENSE b/LICENSE index e7424bbc2..e1c3be202 100644 --- a/LICENSE +++ b/LICENSE @@ -4,11 +4,11 @@ LICENSE.BSD in this directory. GTSAM contains two third party libraries, with documentation of licensing and modifications as follows: -- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree +- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree ordering library - Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig - - http://www.cise.ufl.edu/research/sparse - - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt + - http://faculty.cse.tamu.edu/davis/suitesparse.html + - Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt - Eigen 3.2: General C++ matrix and linear algebra library - Modified with 3 patches that have been contributed back to the Eigen team: - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) diff --git a/README.md b/README.md index 040f1134f..88f69dec4 100644 --- a/README.md +++ b/README.md @@ -1,59 +1,77 @@ -README - Georgia Tech Smoothing and Mapping library -=================================================== - -What is GTSAM? --------------- - -GTSAM is a library of C++ classes that implement smoothing and -mapping (SAM) in robotics and vision, using factor graphs and Bayes -networks as the underlying computing paradigm rather than sparse -matrices. - -On top of the C++ library, GTSAM includes a MATLAB interface (enable -GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface -is under development. - -Quickstart ----------- - -In the root library folder execute: - -``` -#!bash -$ mkdir build -$ cd build -$ cmake .. -$ make check (optional, runs unit tests) -$ make install -``` - -Prerequisites: - -- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) -- A modern compiler, i.e., at least gcc 4.7.3 on Linux. - -Optional prerequisites - used automatically if findable by CMake: - -- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) - -GTSAM 4 Compatibility ---------------------- - -GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. - -Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. - -Additional Information ----------------------- - -Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. - -See the [`INSTALL`](INSTALL) file for more detailed installation instructions. - -GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. - -Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. - +README - Georgia Tech Smoothing and Mapping library +=================================================== + +What is GTSAM? +-------------- + +GTSAM is a library of C++ classes that implement smoothing and +mapping (SAM) in robotics and vision, using factor graphs and Bayes +networks as the underlying computing paradigm rather than sparse +matrices. + +On top of the C++ library, GTSAM includes a MATLAB interface (enable +GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface +is under development. + +Quickstart +---------- + +In the root library folder execute: + +``` +#!bash +$ mkdir build +$ cd build +$ cmake .. +$ make check (optional, runs unit tests) +$ make install +``` + +Prerequisites: + +- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) +- A modern compiler, i.e., at least gcc 4.7.3 on Linux. + +Optional prerequisites - used automatically if findable by CMake: + +- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) + +GTSAM 4 Compatibility +--------------------- + +GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. + +Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. + +The Preintegrated IMU Factor +---------------------------- + +GTSAM includes a state of the art IMU handling scheme based on + +- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + +Our implementation improves on this using integration on the manifold, as detailed in + +- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. +- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. + +If you are using the factor in academic work, please cite the publications above. + +In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF. + + + +Additional Information +---------------------- + +Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. + +See the [`INSTALL`](INSTALL) file for more detailed installation instructions. + +GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. + +Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. + GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml new file mode 100644 index 000000000..2d8b7f061 --- /dev/null +++ b/bitbucket-pipelines.yml @@ -0,0 +1,16 @@ +# This is a sample build configuration for C++ – Make. +# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples. +# Only use spaces to indent your .yml configuration. +# ----- +# You can specify a custom docker image from Docker Hub as your build environment. +image: ilssim/cmake-boost-qt5 + +pipelines: + default: + - step: + script: # Modify the commands below to build your repository. + - mkdir build + - cd build + - cmake .. + - make + - make check \ No newline at end of file diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 96a0f11f3..e9521c257 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -6,7 +6,8 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}") if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.") - set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE}) + set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE} CACHE STRING + "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." FORCE) endif() # Add option for using build type postfixes to allow installing multiple build modes @@ -60,18 +61,18 @@ mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_C # Apply the gtsam specific build flags as normal variables. This makes it so that they only # apply to the gtsam part of the build if gtsam is built as a subproject -set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS}) -set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS}) -set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG}) -set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}) -set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}) -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}) -set(CMAKE_C_FLAGS_RELEASE ${GTSAM_CMAKE_C_FLAGS_RELEASE}) -set(CMAKE_CXX_FLAGS_RELEASE ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}) -set(CMAKE_C_FLAGS_PROFILING ${GTSAM_CMAKE_C_FLAGS_PROFILING}) -set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}) -set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING}) -set(CMAKE_CXX_FLAGS_TIMING ${GTSAM_CMAKE_CXX_FLAGS_TIMING}) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${GTSAM_CMAKE_C_FLAGS}") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GTSAM_CMAKE_CXX_FLAGS}") +set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} ${GTSAM_CMAKE_C_FLAGS_DEBUG}") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}") +set(CMAKE_C_FLAGS_RELWITHDEBINFO "${CMAKE_C_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}") +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}") +set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} ${GTSAM_CMAKE_C_FLAGS_RELEASE}") +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}") +set(CMAKE_C_FLAGS_PROFILING "${CMAKE_C_FLAGS_PROFILING} ${GTSAM_CMAKE_C_FLAGS_PROFILING}") +set(CMAKE_CXX_FLAGS_PROFILING "${CMAKE_CXX_FLAGS_PROFILING} ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}") +set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_TIMING} ${GTSAM_CMAKE_C_FLAGS_TIMING}") +set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_TIMING} ${GTSAM_CMAKE_CXX_FLAGS_TIMING}") set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING}) set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING}) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index d1d3d93dd..a9e04a01a 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -128,8 +128,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex ## This needs to be fixed!! if(UNIX AND NOT APPLE) list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE} - ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE} - ${Boost_REGEX_LIBRARY_RELEASE}) + ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE}) if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0 list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE}) if(GTSAM_MEX_BUILD_STATIC_MODULE) diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index e13888c00..15d4219e6 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -101,6 +101,9 @@ mark_as_advanced(GTSAM_SINGLE_TEST_EXE) # Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) if(GTSAM_BUILD_TESTS) add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) + + # Add target to build tests without running + add_custom_target(all.tests) endif() # Add examples target @@ -109,8 +112,6 @@ add_custom_target(examples) # Add timing target add_custom_target(timing) -# Add target to build tests without running -add_custom_target(all.tests) # Implementations of this file's macros: @@ -179,11 +180,17 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}") endforeach() else() + + #skip folders which don't have any tests + if(NOT script_srcs) + return() + endif() + # Default on MSVC and XCode - combine test group into a single exectuable set(target_name check_${groupName}_program) # Add executable - add_executable(${target_name} ${script_srcs} ${script_headers}) + add_executable(${target_name} "${script_srcs}" ${script_headers}) target_link_libraries(${target_name} CppUnitLite ${linkLibraries}) # Only have a main function in one script - use preprocessor @@ -196,7 +203,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_dependencies(check.${groupName} ${target_name}) add_dependencies(check ${target_name}) if(NOT XCODE_VERSION) - add_dependencies(all.tests ${script_name}) + add_dependencies(all.tests ${target_name}) endif() # Add TOPSRCDIR diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 43814731f..204af1fea 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -48,8 +48,7 @@ public: virtual Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { SimpleCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_); - return reprojectionError.vector(); + return camera.project(P_, H, boost::none, boost::none) - p_; } }; @@ -72,18 +71,14 @@ int main(int argc, char* argv[]) { // add measurement factors SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5)); boost::shared_ptr factor; - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 45), Point3(10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 45), Point3(-10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 55), Point3(-10, -10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 55), Point3(10, -10, 0))); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 45), Point3(10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 45), Point3(-10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 55), Point3(-10, -10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 55), Point3(10, -10, 0)); /* 3. Create an initial estimate for the camera pose */ Values initial; diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 010f474bf..082b4c0f9 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -18,7 +18,6 @@ #include #include -#include #include using namespace boost::assign; @@ -41,7 +40,7 @@ void createExampleBALFile(const string& filename, const vector& P, data.cameras.push_back(SfM_Camera(pose1, K)); data.cameras.push_back(SfM_Camera(pose2, K)); - BOOST_FOREACH(const Point3& p, P) { + for(const Point3& p: P) { // Create the track SfM_Track track; diff --git a/examples/Data/QPExample.QPS b/examples/Data/QPExample.QPS new file mode 100644 index 000000000..d67c59e0a --- /dev/null +++ b/examples/Data/QPExample.QPS @@ -0,0 +1,21 @@ +NAME QP example +ROWS + N obj + G r1 + L r2 +COLUMNS + c1 r1 2.0 r2 -1.0 + c1 obj 1.5 + c2 r1 1.0 r2 2.0 + c2 obj -2.0 +RHS + rhs1 obj -4.0 + rhs1 r1 2.0 r2 6.0 +RANGES +BOUNDS + UP BOUNDS c1 20.0 +QUADOBJ + c1 c1 8.0 + c1 c2 2.0 + c2 c2 10.0 +ENDATA \ No newline at end of file diff --git a/examples/Data/imuAndGPSdata.csv b/examples/Data/imuAndGPSdata.csv new file mode 100644 index 000000000..9631e3374 --- /dev/null +++ b/examples/Data/imuAndGPSdata.csv @@ -0,0 +1,20101 @@ +i,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.000100,0.0,0.0,0.0,0.0,0.0 +1,0.000000,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.000200,0.0,0.0,0.0,0.0,0.0 +0,0.000300,0.0,0.0,0.0,0.0,0.0 +0,0.000400,0.0,0.0,0.0,0.0,0.0 +0,0.000500,0.0,0.0,0.0,0.0,0.0 +0,0.000600,0.0,0.0,0.0,0.0,0.0 +0,0.000700,0.0,0.0,0.0,0.0,0.0 +0,0.000800,0.0,0.0,0.0,0.0,0.0 +0,0.000900,0.0,0.0,0.0,0.0,0.0 +0,0.001000,0.0,0.0,0.0,0.0,0.0 +0,0.001100,0.0,0.0,0.0,0.0,0.0 +0,0.001200,0.0,0.0,0.0,0.0,0.0 +0,0.001300,0.0,0.0,0.0,0.0,0.0 +0,0.001400,0.0,0.0,0.0,0.0,0.0 +0,0.001500,0.0,0.0,0.0,0.0,0.0 +0,0.001600,0.0,0.0,0.0,0.0,0.0 +0,0.001700,0.0,0.0,0.0,0.0,0.0 +0,0.001800,0.0,0.0,0.0,0.0,0.0 +0,0.001900,0.0,0.0,0.0,0.0,0.0 +0,0.002000,0.0,0.0,0.0,0.0,0.0 +0,0.002100,0.0,0.0,0.0,0.0,0.0 +0,0.002200,0.0,0.0,0.0,0.0,0.0 +0,0.002300,0.0,0.0,0.0,0.0,0.0 +0,0.002400,0.0,0.0,0.0,0.0,0.0 +0,0.002500,0.0,0.0,0.0,0.0,0.0 +0,0.002600,0.0,0.0,0.0,0.0,0.0 +0,0.002700,0.0,0.0,0.0,0.0,0.0 +0,0.002800,0.0,0.0,0.0,0.0,0.0 +0,0.002900,0.0,0.0,0.0,0.0,0.0 +0,0.003000,0.0,0.0,0.0,0.0,0.0 +0,0.003100,0.0,0.0,0.0,0.0,0.0 +0,0.003200,0.0,0.0,0.0,0.0,0.0 +0,0.003300,0.0,0.0,0.0,0.0,0.0 +0,0.003400,0.0,0.0,0.0,0.0,0.0 +0,0.003500,0.0,0.0,0.0,0.0,0.0 +0,0.003600,0.0,0.0,0.0,0.0,0.0 +0,0.003700,0.0,0.0,0.0,0.0,0.0 +0,0.003800,0.0,0.0,0.0,0.0,0.0 +0,0.003900,0.0,0.0,0.0,0.0,0.0 +0,0.004000,0.0,0.0,0.0,0.0,0.0 +0,0.004100,0.0,0.0,0.0,0.0,0.0 +0,0.004200,0.0,0.0,0.0,0.0,0.0 +0,0.004300,0.0,0.0,0.0,0.0,0.0 +0,0.004400,0.0,0.0,0.0,0.0,0.0 +0,0.004500,0.0,0.0,0.0,0.0,0.0 +0,0.004600,0.0,0.0,0.0,0.0,0.0 +0,0.004700,0.0,0.0,0.0,0.0,0.0 +0,0.004800,0.0,0.0,0.0,0.0,0.0 +0,0.004900,0.0,0.0,0.0,0.0,0.0 +0,0.005000,0.0,0.0,0.0,0.0,0.0 +0,0.005100,0.0,0.0,0.0,0.0,0.0 +0,0.005200,0.0,0.0,0.0,0.0,0.0 +0,0.005300,0.0,0.0,0.0,0.0,0.0 +0,0.005400,0.0,0.0,0.0,0.0,0.0 +0,0.005500,0.0,0.0,0.0,0.0,0.0 +0,0.005600,0.0,0.0,0.0,0.0,0.0 +0,0.005700,0.0,0.0,0.0,0.0,0.0 +0,0.005800,0.0,0.0,0.0,0.0,0.0 +0,0.005900,0.0,0.0,0.0,0.0,0.0 +0,0.006000,0.0,0.0,0.0,0.0,0.0 +0,0.006100,0.0,0.0,0.0,0.0,0.0 +0,0.006200,0.0,0.0,0.0,0.0,0.0 +0,0.006300,0.0,0.0,0.0,0.0,0.0 +0,0.006400,0.0,0.0,0.0,0.0,0.0 +0,0.006500,0.0,0.0,0.0,0.0,0.0 +0,0.006600,0.0,0.0,0.0,0.0,0.0 +0,0.006700,0.0,0.0,0.0,0.0,0.0 +0,0.006800,0.0,0.0,0.0,0.0,0.0 +0,0.006900,0.0,0.0,0.0,0.0,0.0 +0,0.007000,0.0,0.0,0.0,0.0,0.0 +0,0.007100,0.0,0.0,0.0,0.0,0.0 +0,0.007200,0.0,0.0,0.0,0.0,0.0 +0,0.007300,0.0,0.0,0.0,0.0,0.0 +0,0.007400,0.0,0.0,0.0,0.0,0.0 +0,0.007500,0.0,0.0,0.0,0.0,0.0 +0,0.007600,0.0,0.0,0.0,0.0,0.0 +0,0.007700,0.0,0.0,0.0,0.0,0.0 +0,0.007800,0.0,0.0,0.0,0.0,0.0 +0,0.007900,0.0,0.0,0.0,0.0,0.0 +0,0.008000,0.0,0.0,0.0,0.0,0.0 +0,0.008100,0.0,0.0,0.0,0.0,0.0 +0,0.008200,0.0,0.0,0.0,0.0,0.0 +0,0.008300,0.0,0.0,0.0,0.0,0.0 +0,0.008400,0.0,0.0,0.0,0.0,0.0 +0,0.008500,0.0,0.0,0.0,0.0,0.0 +0,0.008600,0.0,0.0,0.0,0.0,0.0 +0,0.008700,0.0,0.0,0.0,0.0,0.0 +0,0.008800,0.0,0.0,0.0,0.0,0.0 +0,0.008900,0.0,0.0,0.0,0.0,0.0 +0,0.009000,0.0,0.0,0.0,0.0,0.0 +0,0.009100,0.0,0.0,0.0,0.0,0.0 +0,0.009200,0.0,0.0,0.0,0.0,0.0 +0,0.009300,0.0,0.0,0.0,0.0,0.0 +0,0.009400,0.0,0.0,0.0,0.0,0.0 +0,0.009500,0.0,0.0,0.0,0.0,0.0 +0,0.009600,0.0,0.0,0.0,0.0,0.0 +0,0.009700,0.0,0.0,0.0,0.0,0.0 +0,0.009800,0.0,0.0,0.0,0.0,0.0 +0,0.009900,0.0,0.0,0.0,0.0,0.0 +0,0.010000,0.0,0.0,0.0,0.0,0.0 +0,0.010100,0.0,0.0,0.0,0.0,0.0 +0,0.010200,0.0,0.0,0.0,0.0,0.0 +0,0.010300,0.0,0.0,0.0,0.0,0.0 +0,0.010400,0.0,0.0,0.0,0.0,0.0 +0,0.010500,0.0,0.0,0.0,0.0,0.0 +0,0.010600,0.0,0.0,0.0,0.0,0.0 +0,0.010700,0.0,0.0,0.0,0.0,0.0 +0,0.010800,0.0,0.0,0.0,0.0,0.0 +0,0.010900,0.0,0.0,0.0,0.0,0.0 +0,0.011000,0.0,0.0,0.0,0.0,0.0 +0,0.011100,0.0,0.0,0.0,0.0,0.0 +0,0.011200,0.0,0.0,0.0,0.0,0.0 +0,0.011300,0.0,0.0,0.0,0.0,0.0 +0,0.011400,0.0,0.0,0.0,0.0,0.0 +0,0.011500,0.0,0.0,0.0,0.0,0.0 +0,0.011600,0.0,0.0,0.0,0.0,0.0 +0,0.011700,0.0,0.0,0.0,0.0,0.0 +0,0.011800,0.0,0.0,0.0,0.0,0.0 +0,0.011900,0.0,0.0,0.0,0.0,0.0 +0,0.012000,0.0,0.0,0.0,0.0,0.0 +0,0.012100,0.0,0.0,0.0,0.0,0.0 +0,0.012200,0.0,0.0,0.0,0.0,0.0 +0,0.012300,0.0,0.0,0.0,0.0,0.0 +0,0.012400,0.0,0.0,0.0,0.0,0.0 +0,0.012500,0.0,0.0,0.0,0.0,0.0 +0,0.012600,0.0,0.0,0.0,0.0,0.0 +0,0.012700,0.0,0.0,0.0,0.0,0.0 +0,0.012800,0.0,0.0,0.0,0.0,0.0 +0,0.012900,0.0,0.0,0.0,0.0,0.0 +0,0.013000,0.0,0.0,0.0,0.0,0.0 +0,0.013100,0.0,0.0,0.0,0.0,0.0 +0,0.013200,0.0,0.0,0.0,0.0,0.0 +0,0.013300,0.0,0.0,0.0,0.0,0.0 +0,0.013400,0.0,0.0,0.0,0.0,0.0 +0,0.013500,0.0,0.0,0.0,0.0,0.0 +0,0.013600,0.0,0.0,0.0,0.0,0.0 +0,0.013700,0.0,0.0,0.0,0.0,0.0 +0,0.013800,0.0,0.0,0.0,0.0,0.0 +0,0.013900,0.0,0.0,0.0,0.0,0.0 +0,0.014000,0.0,0.0,0.0,0.0,0.0 +0,0.014100,0.0,0.0,0.0,0.0,0.0 +0,0.014200,0.0,0.0,0.0,0.0,0.0 +0,0.014300,0.0,0.0,0.0,0.0,0.0 +0,0.014400,0.0,0.0,0.0,0.0,0.0 +0,0.014500,0.0,0.0,0.0,0.0,0.0 +0,0.014600,0.0,0.0,0.0,0.0,0.0 +0,0.014700,0.0,0.0,0.0,0.0,0.0 +0,0.014800,0.0,0.0,0.0,0.0,0.0 +0,0.014900,0.0,0.0,0.0,0.0,0.0 +0,0.015000,0.0,0.0,0.0,0.0,0.0 +0,0.015100,0.0,0.0,0.0,0.0,0.0 +0,0.015200,0.0,0.0,0.0,0.0,0.0 +0,0.015300,0.0,0.0,0.0,0.0,0.0 +0,0.015400,0.0,0.0,0.0,0.0,0.0 +0,0.015500,0.0,0.0,0.0,0.0,0.0 +0,0.015600,0.0,0.0,0.0,0.0,0.0 +0,0.015700,0.0,0.0,0.0,0.0,0.0 +0,0.015800,0.0,0.0,0.0,0.0,0.0 +0,0.015900,0.0,0.0,0.0,0.0,0.0 +0,0.016000,0.0,0.0,0.0,0.0,0.0 +0,0.016100,0.0,0.0,0.0,0.0,0.0 +0,0.016200,0.0,0.0,0.0,0.0,0.0 +0,0.016300,0.0,0.0,0.0,0.0,0.0 +0,0.016400,0.0,0.0,0.0,0.0,0.0 +0,0.016500,0.0,0.0,0.0,0.0,0.0 +0,0.016600,0.0,0.0,0.0,0.0,0.0 +0,0.016700,0.0,0.0,0.0,0.0,0.0 +0,0.016800,0.0,0.0,0.0,0.0,0.0 +0,0.016900,0.0,0.0,0.0,0.0,0.0 +0,0.017000,0.0,0.0,0.0,0.0,0.0 +0,0.017100,0.0,0.0,0.0,0.0,0.0 +0,0.017200,0.0,0.0,0.0,0.0,0.0 +0,0.017300,0.0,0.0,0.0,0.0,0.0 +0,0.017400,0.0,0.0,0.0,0.0,0.0 +0,0.017500,0.0,0.0,0.0,0.0,0.0 +0,0.017600,0.0,0.0,0.0,0.0,0.0 +0,0.017700,0.0,0.0,0.0,0.0,0.0 +0,0.017800,0.0,0.0,0.0,0.0,0.0 +0,0.017900,0.0,0.0,0.0,0.0,0.0 +0,0.018000,0.0,0.0,0.0,0.0,0.0 +0,0.018100,0.0,0.0,0.0,0.0,0.0 +0,0.018200,0.0,0.0,0.0,0.0,0.0 +0,0.018300,0.0,0.0,0.0,0.0,0.0 +0,0.018400,0.0,0.0,0.0,0.0,0.0 +0,0.018500,0.0,0.0,0.0,0.0,0.0 +0,0.018600,0.0,0.0,0.0,0.0,0.0 +0,0.018700,0.0,0.0,0.0,0.0,0.0 +0,0.018800,0.0,0.0,0.0,0.0,0.0 +0,0.018900,0.0,0.0,0.0,0.0,0.0 +0,0.019000,0.0,0.0,0.0,0.0,0.0 +0,0.019100,0.0,0.0,0.0,0.0,0.0 +0,0.019200,0.0,0.0,0.0,0.0,0.0 +0,0.019300,0.0,0.0,0.0,0.0,0.0 +0,0.019400,0.0,0.0,0.0,0.0,0.0 +0,0.019500,0.0,0.0,0.0,0.0,0.0 +0,0.019600,0.0,0.0,0.0,0.0,0.0 +0,0.019700,0.0,0.0,0.0,0.0,0.0 +0,0.019800,0.0,0.0,0.0,0.0,0.0 +0,0.019900,0.0,0.0,0.0,0.0,0.0 +0,0.020000,0.0,0.0,0.0,0.0,0.0 +0,0.020100,0.0,0.0,0.0,0.0,0.0 +1,0.003409,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.020200,0.0,0.0,0.0,0.0,0.0 +0,0.020300,0.0,0.0,0.0,0.0,0.0 +0,0.020400,0.0,0.0,0.0,0.0,0.0 +0,0.020500,0.0,0.0,0.0,0.0,0.0 +0,0.020600,0.0,0.0,0.0,0.0,0.0 +0,0.020700,0.0,0.0,0.0,0.0,0.0 +0,0.020800,0.0,0.0,0.0,0.0,0.0 +0,0.020900,0.0,0.0,0.0,0.0,0.0 +0,0.021000,0.0,0.0,0.0,0.0,0.0 +0,0.021100,0.0,0.0,0.0,0.0,0.0 +0,0.021200,0.0,0.0,0.0,0.0,0.0 +0,0.021300,0.0,0.0,0.0,0.0,0.0 +0,0.021400,0.0,0.0,0.0,0.0,0.0 +0,0.021500,0.0,0.0,0.0,0.0,0.0 +0,0.021600,0.0,0.0,0.0,0.0,0.0 +0,0.021700,0.0,0.0,0.0,0.0,0.0 +0,0.021800,0.0,0.0,0.0,0.0,0.0 +0,0.021900,0.0,0.0,0.0,0.0,0.0 +0,0.022000,0.0,0.0,0.0,0.0,0.0 +0,0.022100,0.0,0.0,0.0,0.0,0.0 +0,0.022200,0.0,0.0,0.0,0.0,0.0 +0,0.022300,0.0,0.0,0.0,0.0,0.0 +0,0.022400,0.0,0.0,0.0,0.0,0.0 +0,0.022500,0.0,0.0,0.0,0.0,0.0 +0,0.022600,0.0,0.0,0.0,0.0,0.0 +0,0.022700,0.0,0.0,0.0,0.0,0.0 +0,0.022800,0.0,0.0,0.0,0.0,0.0 +0,0.022900,0.0,0.0,0.0,0.0,0.0 +0,0.023000,0.0,0.0,0.0,0.0,0.0 +0,0.023100,0.0,0.0,0.0,0.0,0.0 +0,0.023200,0.0,0.0,0.0,0.0,0.0 +0,0.023300,0.0,0.0,0.0,0.0,0.0 +0,0.023400,0.0,0.0,0.0,0.0,0.0 +0,0.023500,0.0,0.0,0.0,0.0,0.0 +0,0.023600,0.0,0.0,0.0,0.0,0.0 +0,0.023700,0.0,0.0,0.0,0.0,0.0 +0,0.023800,0.0,0.0,0.0,0.0,0.0 +0,0.023900,0.0,0.0,0.0,0.0,0.0 +0,0.024000,0.0,0.0,0.0,0.0,0.0 +0,0.024100,0.0,0.0,0.0,0.0,0.0 +0,0.024200,0.0,0.0,0.0,0.0,0.0 +0,0.024300,0.0,0.0,0.0,0.0,0.0 +0,0.024400,0.0,0.0,0.0,0.0,0.0 +0,0.024500,0.0,0.0,0.0,0.0,0.0 +0,0.024600,0.0,0.0,0.0,0.0,0.0 +0,0.024700,0.0,0.0,0.0,0.0,0.0 +0,0.024800,0.0,0.0,0.0,0.0,0.0 +0,0.024900,0.0,0.0,0.0,0.0,0.0 +0,0.025000,0.0,0.0,0.0,0.0,0.0 +0,0.025100,0.0,0.0,0.0,0.0,0.0 +0,0.025200,0.0,0.0,0.0,0.0,0.0 +0,0.025300,0.0,0.0,0.0,0.0,0.0 +0,0.025400,0.0,0.0,0.0,0.0,0.0 +0,0.025500,0.0,0.0,0.0,0.0,0.0 +0,0.025600,0.0,0.0,0.0,0.0,0.0 +0,0.025700,0.0,0.0,0.0,0.0,0.0 +0,0.025800,0.0,0.0,0.0,0.0,0.0 +0,0.025900,0.0,0.0,0.0,0.0,0.0 +0,0.026000,0.0,0.0,0.0,0.0,0.0 +0,0.026100,0.0,0.0,0.0,0.0,0.0 +0,0.026200,0.0,0.0,0.0,0.0,0.0 +0,0.026300,0.0,0.0,0.0,0.0,0.0 +0,0.026400,0.0,0.0,0.0,0.0,0.0 +0,0.026500,0.0,0.0,0.0,0.0,0.0 +0,0.026600,0.0,0.0,0.0,0.0,0.0 +0,0.026700,0.0,0.0,0.0,0.0,0.0 +0,0.026800,0.0,0.0,0.0,0.0,0.0 +0,0.026900,0.0,0.0,0.0,0.0,0.0 +0,0.027000,0.0,0.0,0.0,0.0,0.0 +0,0.027100,0.0,0.0,0.0,0.0,0.0 +0,0.027200,0.0,0.0,0.0,0.0,0.0 +0,0.027300,0.0,0.0,0.0,0.0,0.0 +0,0.027400,0.0,0.0,0.0,0.0,0.0 +0,0.027500,0.0,0.0,0.0,0.0,0.0 +0,0.027600,0.0,0.0,0.0,0.0,0.0 +0,0.027700,0.0,0.0,0.0,0.0,0.0 +0,0.027800,0.0,0.0,0.0,0.0,0.0 +0,0.027900,0.0,0.0,0.0,0.0,0.0 +0,0.028000,0.0,0.0,0.0,0.0,0.0 +0,0.028100,0.0,0.0,0.0,0.0,0.0 +0,0.028200,0.0,0.0,0.0,0.0,0.0 +0,0.028300,0.0,0.0,0.0,0.0,0.0 +0,0.028400,0.0,0.0,0.0,0.0,0.0 +0,0.028500,0.0,0.0,0.0,0.0,0.0 +0,0.028600,0.0,0.0,0.0,0.0,0.0 +0,0.028700,0.0,0.0,0.0,0.0,0.0 +0,0.028800,0.0,0.0,0.0,0.0,0.0 +0,0.028900,0.0,0.0,0.0,0.0,0.0 +0,0.029000,0.0,0.0,0.0,0.0,0.0 +0,0.029100,0.0,0.0,0.0,0.0,0.0 +0,0.029200,0.0,0.0,0.0,0.0,0.0 +0,0.029300,0.0,0.0,0.0,0.0,0.0 +0,0.029400,0.0,0.0,0.0,0.0,0.0 +0,0.029500,0.0,0.0,0.0,0.0,0.0 +0,0.029600,0.0,0.0,0.0,0.0,0.0 +0,0.029700,0.0,0.0,0.0,0.0,0.0 +0,0.029800,0.0,0.0,0.0,0.0,0.0 +0,0.029900,0.0,0.0,0.0,0.0,0.0 +0,0.030000,0.0,0.0,0.0,0.0,0.0 +0,0.030100,0.0,0.0,0.0,0.0,0.0 +0,0.030200,0.0,0.0,0.0,0.0,0.0 +0,0.030300,0.0,0.0,0.0,0.0,0.0 +0,0.030400,0.0,0.0,0.0,0.0,0.0 +0,0.030500,0.0,0.0,0.0,0.0,0.0 +0,0.030600,0.0,0.0,0.0,0.0,0.0 +0,0.030700,0.0,0.0,0.0,0.0,0.0 +0,0.030800,0.0,0.0,0.0,0.0,0.0 +0,0.030900,0.0,0.0,0.0,0.0,0.0 +0,0.031000,0.0,0.0,0.0,0.0,0.0 +0,0.031100,0.0,0.0,0.0,0.0,0.0 +0,0.031200,0.0,0.0,0.0,0.0,0.0 +0,0.031300,0.0,0.0,0.0,0.0,0.0 +0,0.031400,0.0,0.0,0.0,0.0,0.0 +0,0.031500,0.0,0.0,0.0,0.0,0.0 +0,0.031600,0.0,0.0,0.0,0.0,0.0 +0,0.031700,0.0,0.0,0.0,0.0,0.0 +0,0.031800,0.0,0.0,0.0,0.0,0.0 +0,0.031900,0.0,0.0,0.0,0.0,0.0 +0,0.032000,0.0,0.0,0.0,0.0,0.0 +0,0.032100,0.0,0.0,0.0,0.0,0.0 +0,0.032200,0.0,0.0,0.0,0.0,0.0 +0,0.032300,0.0,0.0,0.0,0.0,0.0 +0,0.032400,0.0,0.0,0.0,0.0,0.0 +0,0.032500,0.0,0.0,0.0,0.0,0.0 +0,0.032600,0.0,0.0,0.0,0.0,0.0 +0,0.032700,0.0,0.0,0.0,0.0,0.0 +0,0.032800,0.0,0.0,0.0,0.0,0.0 +0,0.032900,0.0,0.0,0.0,0.0,0.0 +0,0.033000,0.0,0.0,0.0,0.0,0.0 +0,0.033100,0.0,0.0,0.0,0.0,0.0 +0,0.033200,0.0,0.0,0.0,0.0,0.0 +0,0.033300,0.0,0.0,0.0,0.0,0.0 +0,0.033400,0.0,0.0,0.0,0.0,0.0 +0,0.033500,0.0,0.0,0.0,0.0,0.0 +0,0.033600,0.0,0.0,0.0,0.0,0.0 +0,0.033700,0.0,0.0,0.0,0.0,0.0 +0,0.033800,0.0,0.0,0.0,0.0,0.0 +0,0.033900,0.0,0.0,0.0,0.0,0.0 +0,0.034000,0.0,0.0,0.0,0.0,0.0 +0,0.034100,0.0,0.0,0.0,0.0,0.0 +0,0.034200,0.0,0.0,0.0,0.0,0.0 +0,0.034300,0.0,0.0,0.0,0.0,0.0 +0,0.034400,0.0,0.0,0.0,0.0,0.0 +0,0.034500,0.0,0.0,0.0,0.0,0.0 +0,0.034600,0.0,0.0,0.0,0.0,0.0 +0,0.034700,0.0,0.0,0.0,0.0,0.0 +0,0.034800,0.0,0.0,0.0,0.0,0.0 +0,0.034900,0.0,0.0,0.0,0.0,0.0 +0,0.035000,0.0,0.0,0.0,0.0,0.0 +0,0.035100,0.0,0.0,0.0,0.0,0.0 +0,0.035200,0.0,0.0,0.0,0.0,0.0 +0,0.035300,0.0,0.0,0.0,0.0,0.0 +0,0.035400,0.0,0.0,0.0,0.0,0.0 +0,0.035500,0.0,0.0,0.0,0.0,0.0 +0,0.035600,0.0,0.0,0.0,0.0,0.0 +0,0.035700,0.0,0.0,0.0,0.0,0.0 +0,0.035800,0.0,0.0,0.0,0.0,0.0 +0,0.035900,0.0,0.0,0.0,0.0,0.0 +0,0.036000,0.0,0.0,0.0,0.0,0.0 +0,0.036100,0.0,0.0,0.0,0.0,0.0 +0,0.036200,0.0,0.0,0.0,0.0,0.0 +0,0.036300,0.0,0.0,0.0,0.0,0.0 +0,0.036400,0.0,0.0,0.0,0.0,0.0 +0,0.036500,0.0,0.0,0.0,0.0,0.0 +0,0.036600,0.0,0.0,0.0,0.0,0.0 +0,0.036700,0.0,0.0,0.0,0.0,0.0 +0,0.036800,0.0,0.0,0.0,0.0,0.0 +0,0.036900,0.0,0.0,0.0,0.0,0.0 +0,0.037000,0.0,0.0,0.0,0.0,0.0 +0,0.037100,0.0,0.0,0.0,0.0,0.0 +0,0.037200,0.0,0.0,0.0,0.0,0.0 +0,0.037300,0.0,0.0,0.0,0.0,0.0 +0,0.037400,0.0,0.0,0.0,0.0,0.0 +0,0.037500,0.0,0.0,0.0,0.0,0.0 +0,0.037600,0.0,0.0,0.0,0.0,0.0 +0,0.037700,0.0,0.0,0.0,0.0,0.0 +0,0.037800,0.0,0.0,0.0,0.0,0.0 +0,0.037900,0.0,0.0,0.0,0.0,0.0 +0,0.038000,0.0,0.0,0.0,0.0,0.0 +0,0.038100,0.0,0.0,0.0,0.0,0.0 +0,0.038200,0.0,0.0,0.0,0.0,0.0 +0,0.038300,0.0,0.0,0.0,0.0,0.0 +0,0.038400,0.0,0.0,0.0,0.0,0.0 +0,0.038500,0.0,0.0,0.0,0.0,0.0 +0,0.038600,0.0,0.0,0.0,0.0,0.0 +0,0.038700,0.0,0.0,0.0,0.0,0.0 +0,0.038800,0.0,0.0,0.0,0.0,0.0 +0,0.038900,0.0,0.0,0.0,0.0,0.0 +0,0.039000,0.0,0.0,0.0,0.0,0.0 +0,0.039100,0.0,0.0,0.0,0.0,0.0 +0,0.039200,0.0,0.0,0.0,0.0,0.0 +0,0.039300,0.0,0.0,0.0,0.0,0.0 +0,0.039400,0.0,0.0,0.0,0.0,0.0 +0,0.039500,0.0,0.0,0.0,0.0,0.0 +0,0.039600,0.0,0.0,0.0,0.0,0.0 +0,0.039700,0.0,0.0,0.0,0.0,0.0 +0,0.039800,0.0,0.0,0.0,0.0,0.0 +0,0.039900,0.0,0.0,0.0,0.0,0.0 +0,0.040000,0.0,0.0,0.0,0.0,0.0 +0,0.040100,0.0,0.0,0.0,0.0,0.0 +1,0.026968,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.040200,0.0,0.0,0.0,0.0,0.0 +0,0.040300,0.0,0.0,0.0,0.0,0.0 +0,0.040400,0.0,0.0,0.0,0.0,0.0 +0,0.040500,0.0,0.0,0.0,0.0,0.0 +0,0.040600,0.0,0.0,0.0,0.0,0.0 +0,0.040700,0.0,0.0,0.0,0.0,0.0 +0,0.040800,0.0,0.0,0.0,0.0,0.0 +0,0.040900,0.0,0.0,0.0,0.0,0.0 +0,0.041000,0.0,0.0,0.0,0.0,0.0 +0,0.041100,0.0,0.0,0.0,0.0,0.0 +0,0.041200,0.0,0.0,0.0,0.0,0.0 +0,0.041300,0.0,0.0,0.0,0.0,0.0 +0,0.041400,0.0,0.0,0.0,0.0,0.0 +0,0.041500,0.0,0.0,0.0,0.0,0.0 +0,0.041600,0.0,0.0,0.0,0.0,0.0 +0,0.041700,0.0,0.0,0.0,0.0,0.0 +0,0.041800,0.0,0.0,0.0,0.0,0.0 +0,0.041900,0.0,0.0,0.0,0.0,0.0 +0,0.042000,0.0,0.0,0.0,0.0,0.0 +0,0.042100,0.0,0.0,0.0,0.0,0.0 +0,0.042200,0.0,0.0,0.0,0.0,0.0 +0,0.042300,0.0,0.0,0.0,0.0,0.0 +0,0.042400,0.0,0.0,0.0,0.0,0.0 +0,0.042500,0.0,0.0,0.0,0.0,0.0 +0,0.042600,0.0,0.0,0.0,0.0,0.0 +0,0.042700,0.0,0.0,0.0,0.0,0.0 +0,0.042800,0.0,0.0,0.0,0.0,0.0 +0,0.042900,0.0,0.0,0.0,0.0,0.0 +0,0.043000,0.0,0.0,0.0,0.0,0.0 +0,0.043100,0.0,0.0,0.0,0.0,0.0 +0,0.043200,0.0,0.0,0.0,0.0,0.0 +0,0.043300,0.0,0.0,0.0,0.0,0.0 +0,0.043400,0.0,0.0,0.0,0.0,0.0 +0,0.043500,0.0,0.0,0.0,0.0,0.0 +0,0.043600,0.0,0.0,0.0,0.0,0.0 +0,0.043700,0.0,0.0,0.0,0.0,0.0 +0,0.043800,0.0,0.0,0.0,0.0,0.0 +0,0.043900,0.0,0.0,0.0,0.0,0.0 +0,0.044000,0.0,0.0,0.0,0.0,0.0 +0,0.044100,0.0,0.0,0.0,0.0,0.0 +0,0.044200,0.0,0.0,0.0,0.0,0.0 +0,0.044300,0.0,0.0,0.0,0.0,0.0 +0,0.044400,0.0,0.0,0.0,0.0,0.0 +0,0.044500,0.0,0.0,0.0,0.0,0.0 +0,0.044600,0.0,0.0,0.0,0.0,0.0 +0,0.044700,0.0,0.0,0.0,0.0,0.0 +0,0.044800,0.0,0.0,0.0,0.0,0.0 +0,0.044900,0.0,0.0,0.0,0.0,0.0 +0,0.045000,0.0,0.0,0.0,0.0,0.0 +0,0.045100,0.0,0.0,0.0,0.0,0.0 +0,0.045200,0.0,0.0,0.0,0.0,0.0 +0,0.045300,0.0,0.0,0.0,0.0,0.0 +0,0.045400,0.0,0.0,0.0,0.0,0.0 +0,0.045500,0.0,0.0,0.0,0.0,0.0 +0,0.045600,0.0,0.0,0.0,0.0,0.0 +0,0.045700,0.0,0.0,0.0,0.0,0.0 +0,0.045800,0.0,0.0,0.0,0.0,0.0 +0,0.045900,0.0,0.0,0.0,0.0,0.0 +0,0.046000,0.0,0.0,0.0,0.0,0.0 +0,0.046100,0.0,0.0,0.0,0.0,0.0 +0,0.046200,0.0,0.0,0.0,0.0,0.0 +0,0.046300,0.0,0.0,0.0,0.0,0.0 +0,0.046400,0.0,0.0,0.0,0.0,0.0 +0,0.046500,0.0,0.0,0.0,0.0,0.0 +0,0.046600,0.0,0.0,0.0,0.0,0.0 +0,0.046700,0.0,0.0,0.0,0.0,0.0 +0,0.046800,0.0,0.0,0.0,0.0,0.0 +0,0.046900,0.0,0.0,0.0,0.0,0.0 +0,0.047000,0.0,0.0,0.0,0.0,0.0 +0,0.047100,0.0,0.0,0.0,0.0,0.0 +0,0.047200,0.0,0.0,0.0,0.0,0.0 +0,0.047300,0.0,0.0,0.0,0.0,0.0 +0,0.047400,0.0,0.0,0.0,0.0,0.0 +0,0.047500,0.0,0.0,0.0,0.0,0.0 +0,0.047600,0.0,0.0,0.0,0.0,0.0 +0,0.047700,0.0,0.0,0.0,0.0,0.0 +0,0.047800,0.0,0.0,0.0,0.0,0.0 +0,0.047900,0.0,0.0,0.0,0.0,0.0 +0,0.048000,0.0,0.0,0.0,0.0,0.0 +0,0.048100,0.0,0.0,0.0,0.0,0.0 +0,0.048200,0.0,0.0,0.0,0.0,0.0 +0,0.048300,0.0,0.0,0.0,0.0,0.0 +0,0.048400,0.0,0.0,0.0,0.0,0.0 +0,0.048500,0.0,0.0,0.0,0.0,0.0 +0,0.048600,0.0,0.0,0.0,0.0,0.0 +0,0.048700,0.0,0.0,0.0,0.0,0.0 +0,0.048800,0.0,0.0,0.0,0.0,0.0 +0,0.048900,0.0,0.0,0.0,0.0,0.0 +0,0.049000,0.0,0.0,0.0,0.0,0.0 +0,0.049100,0.0,0.0,0.0,0.0,0.0 +0,0.049200,0.0,0.0,0.0,0.0,0.0 +0,0.049300,0.0,0.0,0.0,0.0,0.0 +0,0.049400,0.0,0.0,0.0,0.0,0.0 +0,0.049500,0.0,0.0,0.0,0.0,0.0 +0,0.049600,0.0,0.0,0.0,0.0,0.0 +0,0.049700,0.0,0.0,0.0,0.0,0.0 +0,0.049800,0.0,0.0,0.0,0.0,0.0 +0,0.049900,0.0,0.0,0.0,0.0,0.0 +0,0.050000,0.0,0.0,0.0,0.0,0.0 +0,0.050100,0.0,0.0,0.0,0.0,0.0 +0,0.050200,0.0,0.0,0.0,0.0,0.0 +0,0.050300,0.0,0.0,0.0,0.0,0.0 +0,0.050400,0.0,0.0,0.0,0.0,0.0 +0,0.050500,0.0,0.0,0.0,0.0,0.0 +0,0.050600,0.0,0.0,0.0,0.0,0.0 +0,0.050700,0.0,0.0,0.0,0.0,0.0 +0,0.050800,0.0,0.0,0.0,0.0,0.0 +0,0.050900,0.0,0.0,0.0,0.0,0.0 +0,0.051000,0.0,0.0,0.0,0.0,0.0 +0,0.051100,0.0,0.0,0.0,0.0,0.0 +0,0.051200,0.0,0.0,0.0,0.0,0.0 +0,0.051300,0.0,0.0,0.0,0.0,0.0 +0,0.051400,0.0,0.0,0.0,0.0,0.0 +0,0.051500,0.0,0.0,0.0,0.0,0.0 +0,0.051600,0.0,0.0,0.0,0.0,0.0 +0,0.051700,0.0,0.0,0.0,0.0,0.0 +0,0.051800,0.0,0.0,0.0,0.0,0.0 +0,0.051900,0.0,0.0,0.0,0.0,0.0 +0,0.052000,0.0,0.0,0.0,0.0,0.0 +0,0.052100,0.0,0.0,0.0,0.0,0.0 +0,0.052200,0.0,0.0,0.0,0.0,0.0 +0,0.052300,0.0,0.0,0.0,0.0,0.0 +0,0.052400,0.0,0.0,0.0,0.0,0.0 +0,0.052500,0.0,0.0,0.0,0.0,0.0 +0,0.052600,0.0,0.0,0.0,0.0,0.0 +0,0.052700,0.0,0.0,0.0,0.0,0.0 +0,0.052800,0.0,0.0,0.0,0.0,0.0 +0,0.052900,0.0,0.0,0.0,0.0,0.0 +0,0.053000,0.0,0.0,0.0,0.0,0.0 +0,0.053100,0.0,0.0,0.0,0.0,0.0 +0,0.053200,0.0,0.0,0.0,0.0,0.0 +0,0.053300,0.0,0.0,0.0,0.0,0.0 +0,0.053400,0.0,0.0,0.0,0.0,0.0 +0,0.053500,0.0,0.0,0.0,0.0,0.0 +0,0.053600,0.0,0.0,0.0,0.0,0.0 +0,0.053700,0.0,0.0,0.0,0.0,0.0 +0,0.053800,0.0,0.0,0.0,0.0,0.0 +0,0.053900,0.0,0.0,0.0,0.0,0.0 +0,0.054000,0.0,0.0,0.0,0.0,0.0 +0,0.054100,0.0,0.0,0.0,0.0,0.0 +0,0.054200,0.0,0.0,0.0,0.0,0.0 +0,0.054300,0.0,0.0,0.0,0.0,0.0 +0,0.054400,0.0,0.0,0.0,0.0,0.0 +0,0.054500,0.0,0.0,0.0,0.0,0.0 +0,0.054600,0.0,0.0,0.0,0.0,0.0 +0,0.054700,0.0,0.0,0.0,0.0,0.0 +0,0.054800,0.0,0.0,0.0,0.0,0.0 +0,0.054900,0.0,0.0,0.0,0.0,0.0 +0,0.055000,0.0,0.0,0.0,0.0,0.0 +0,0.055100,0.0,0.0,0.0,0.0,0.0 +0,0.055200,0.0,0.0,0.0,0.0,0.0 +0,0.055300,0.0,0.0,0.0,0.0,0.0 +0,0.055400,0.0,0.0,0.0,0.0,0.0 +0,0.055500,0.0,0.0,0.0,0.0,0.0 +0,0.055600,0.0,0.0,0.0,0.0,0.0 +0,0.055700,0.0,0.0,0.0,0.0,0.0 +0,0.055800,0.0,0.0,0.0,0.0,0.0 +0,0.055900,0.0,0.0,0.0,0.0,0.0 +0,0.056000,0.0,0.0,0.0,0.0,0.0 +0,0.056100,0.0,0.0,0.0,0.0,0.0 +0,0.056200,0.0,0.0,0.0,0.0,0.0 +0,0.056300,0.0,0.0,0.0,0.0,0.0 +0,0.056400,0.0,0.0,0.0,0.0,0.0 +0,0.056500,0.0,0.0,0.0,0.0,0.0 +0,0.056600,0.0,0.0,0.0,0.0,0.0 +0,0.056700,0.0,0.0,0.0,0.0,0.0 +0,0.056800,0.0,0.0,0.0,0.0,0.0 +0,0.056900,0.0,0.0,0.0,0.0,0.0 +0,0.057000,0.0,0.0,0.0,0.0,0.0 +0,0.057100,0.0,0.0,0.0,0.0,0.0 +0,0.057200,0.0,0.0,0.0,0.0,0.0 +0,0.057300,0.0,0.0,0.0,0.0,0.0 +0,0.057400,0.0,0.0,0.0,0.0,0.0 +0,0.057500,0.0,0.0,0.0,0.0,0.0 +0,0.057600,0.0,0.0,0.0,0.0,0.0 +0,0.057700,0.0,0.0,0.0,0.0,0.0 +0,0.057800,0.0,0.0,0.0,0.0,0.0 +0,0.057900,0.0,0.0,0.0,0.0,0.0 +0,0.058000,0.0,0.0,0.0,0.0,0.0 +0,0.058100,0.0,0.0,0.0,0.0,0.0 +0,0.058200,0.0,0.0,0.0,0.0,0.0 +0,0.058300,0.0,0.0,0.0,0.0,0.0 +0,0.058400,0.0,0.0,0.0,0.0,0.0 +0,0.058500,0.0,0.0,0.0,0.0,0.0 +0,0.058600,0.0,0.0,0.0,0.0,0.0 +0,0.058700,0.0,0.0,0.0,0.0,0.0 +0,0.058800,0.0,0.0,0.0,0.0,0.0 +0,0.058900,0.0,0.0,0.0,0.0,0.0 +0,0.059000,0.0,0.0,0.0,0.0,0.0 +0,0.059100,0.0,0.0,0.0,0.0,0.0 +0,0.059200,0.0,0.0,0.0,0.0,0.0 +0,0.059300,0.0,0.0,0.0,0.0,0.0 +0,0.059400,0.0,0.0,0.0,0.0,0.0 +0,0.059500,0.0,0.0,0.0,0.0,0.0 +0,0.059600,0.0,0.0,0.0,0.0,0.0 +0,0.059700,0.0,0.0,0.0,0.0,0.0 +0,0.059800,0.0,0.0,0.0,0.0,0.0 +0,0.059900,0.0,0.0,0.0,0.0,0.0 +0,0.060000,0.0,0.0,0.0,0.0,0.0 +0,0.060100,0.0,0.0,0.0,0.0,0.0 +1,0.090677,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.060200,0.0,0.0,0.0,0.0,0.0 +0,0.060300,0.0,0.0,0.0,0.0,0.0 +0,0.060400,0.0,0.0,0.0,0.0,0.0 +0,0.060500,0.0,0.0,0.0,0.0,0.0 +0,0.060600,0.0,0.0,0.0,0.0,0.0 +0,0.060700,0.0,0.0,0.0,0.0,0.0 +0,0.060800,0.0,0.0,0.0,0.0,0.0 +0,0.060900,0.0,0.0,0.0,0.0,0.0 +0,0.061000,0.0,0.0,0.0,0.0,0.0 +0,0.061100,0.0,0.0,0.0,0.0,0.0 +0,0.061200,0.0,0.0,0.0,0.0,0.0 +0,0.061300,0.0,0.0,0.0,0.0,0.0 +0,0.061400,0.0,0.0,0.0,0.0,0.0 +0,0.061500,0.0,0.0,0.0,0.0,0.0 +0,0.061600,0.0,0.0,0.0,0.0,0.0 +0,0.061700,0.0,0.0,0.0,0.0,0.0 +0,0.061800,0.0,0.0,0.0,0.0,0.0 +0,0.061900,0.0,0.0,0.0,0.0,0.0 +0,0.062000,0.0,0.0,0.0,0.0,0.0 +0,0.062100,0.0,0.0,0.0,0.0,0.0 +0,0.062200,0.0,0.0,0.0,0.0,0.0 +0,0.062300,0.0,0.0,0.0,0.0,0.0 +0,0.062400,0.0,0.0,0.0,0.0,0.0 +0,0.062500,0.0,0.0,0.0,0.0,0.0 +0,0.062600,0.0,0.0,0.0,0.0,0.0 +0,0.062700,0.0,0.0,0.0,0.0,0.0 +0,0.062800,0.0,0.0,0.0,0.0,0.0 +0,0.062900,0.0,0.0,0.0,0.0,0.0 +0,0.063000,0.0,0.0,0.0,0.0,0.0 +0,0.063100,0.0,0.0,0.0,0.0,0.0 +0,0.063200,0.0,0.0,0.0,0.0,0.0 +0,0.063300,0.0,0.0,0.0,0.0,0.0 +0,0.063400,0.0,0.0,0.0,0.0,0.0 +0,0.063500,0.0,0.0,0.0,0.0,0.0 +0,0.063600,0.0,0.0,0.0,0.0,0.0 +0,0.063700,0.0,0.0,0.0,0.0,0.0 +0,0.063800,0.0,0.0,0.0,0.0,0.0 +0,0.063900,0.0,0.0,0.0,0.0,0.0 +0,0.064000,0.0,0.0,0.0,0.0,0.0 +0,0.064100,0.0,0.0,0.0,0.0,0.0 +0,0.064200,0.0,0.0,0.0,0.0,0.0 +0,0.064300,0.0,0.0,0.0,0.0,0.0 +0,0.064400,0.0,0.0,0.0,0.0,0.0 +0,0.064500,0.0,0.0,0.0,0.0,0.0 +0,0.064600,0.0,0.0,0.0,0.0,0.0 +0,0.064700,0.0,0.0,0.0,0.0,0.0 +0,0.064800,0.0,0.0,0.0,0.0,0.0 +0,0.064900,0.0,0.0,0.0,0.0,0.0 +0,0.065000,0.0,0.0,0.0,0.0,0.0 +0,0.065100,0.0,0.0,0.0,0.0,0.0 +0,0.065200,0.0,0.0,0.0,0.0,0.0 +0,0.065300,0.0,0.0,0.0,0.0,0.0 +0,0.065400,0.0,0.0,0.0,0.0,0.0 +0,0.065500,0.0,0.0,0.0,0.0,0.0 +0,0.065600,0.0,0.0,0.0,0.0,0.0 +0,0.065700,0.0,0.0,0.0,0.0,0.0 +0,0.065800,0.0,0.0,0.0,0.0,0.0 +0,0.065900,0.0,0.0,0.0,0.0,0.0 +0,0.066000,0.0,0.0,0.0,0.0,0.0 +0,0.066100,0.0,0.0,0.0,0.0,0.0 +0,0.066200,0.0,0.0,0.0,0.0,0.0 +0,0.066300,0.0,0.0,0.0,0.0,0.0 +0,0.066400,0.0,0.0,0.0,0.0,0.0 +0,0.066500,0.0,0.0,0.0,0.0,0.0 +0,0.066600,0.0,0.0,0.0,0.0,0.0 +0,0.066700,0.0,0.0,0.0,0.0,0.0 +0,0.066800,0.0,0.0,0.0,0.0,0.0 +0,0.066900,0.0,0.0,0.0,0.0,0.0 +0,0.067000,0.0,0.0,0.0,0.0,0.0 +0,0.067100,0.0,0.0,0.0,0.0,0.0 +0,0.067200,0.0,0.0,0.0,0.0,0.0 +0,0.067300,0.0,0.0,0.0,0.0,0.0 +0,0.067400,0.0,0.0,0.0,0.0,0.0 +0,0.067500,0.0,0.0,0.0,0.0,0.0 +0,0.067600,0.0,0.0,0.0,0.0,0.0 +0,0.067700,0.0,0.0,0.0,0.0,0.0 +0,0.067800,0.0,0.0,0.0,0.0,0.0 +0,0.067900,0.0,0.0,0.0,0.0,0.0 +0,0.068000,0.0,0.0,0.0,0.0,0.0 +0,0.068100,0.0,0.0,0.0,0.0,0.0 +0,0.068200,0.0,0.0,0.0,0.0,0.0 +0,0.068300,0.0,0.0,0.0,0.0,0.0 +0,0.068400,0.0,0.0,0.0,0.0,0.0 +0,0.068500,0.0,0.0,0.0,0.0,0.0 +0,0.068600,0.0,0.0,0.0,0.0,0.0 +0,0.068700,0.0,0.0,0.0,0.0,0.0 +0,0.068800,0.0,0.0,0.0,0.0,0.0 +0,0.068900,0.0,0.0,0.0,0.0,0.0 +0,0.069000,0.0,0.0,0.0,0.0,0.0 +0,0.069100,0.0,0.0,0.0,0.0,0.0 +0,0.069200,0.0,0.0,0.0,0.0,0.0 +0,0.069300,0.0,0.0,0.0,0.0,0.0 +0,0.069400,0.0,0.0,0.0,0.0,0.0 +0,0.069500,0.0,0.0,0.0,0.0,0.0 +0,0.069600,0.0,0.0,0.0,0.0,0.0 +0,0.069700,0.0,0.0,0.0,0.0,0.0 +0,0.069800,0.0,0.0,0.0,0.0,0.0 +0,0.069900,0.0,0.0,0.0,0.0,0.0 +0,0.070000,0.0,0.0,0.0,0.0,0.0 +0,0.070100,0.0,0.0,0.0,0.0,0.0 +0,0.070200,0.0,0.0,0.0,0.0,0.0 +0,0.070300,0.0,0.0,0.0,0.0,0.0 +0,0.070400,0.0,0.0,0.0,0.0,0.0 +0,0.070500,0.0,0.0,0.0,0.0,0.0 +0,0.070600,0.0,0.0,0.0,0.0,0.0 +0,0.070700,0.0,0.0,0.0,0.0,0.0 +0,0.070800,0.0,0.0,0.0,0.0,0.0 +0,0.070900,0.0,0.0,0.0,0.0,0.0 +0,0.071000,0.0,0.0,0.0,0.0,0.0 +0,0.071100,0.0,0.0,0.0,0.0,0.0 +0,0.071200,0.0,0.0,0.0,0.0,0.0 +0,0.071300,0.0,0.0,0.0,0.0,0.0 +0,0.071400,0.0,0.0,0.0,0.0,0.0 +0,0.071500,0.0,0.0,0.0,0.0,0.0 +0,0.071600,0.0,0.0,0.0,0.0,0.0 +0,0.071700,0.0,0.0,0.0,0.0,0.0 +0,0.071800,0.0,0.0,0.0,0.0,0.0 +0,0.071900,0.0,0.0,0.0,0.0,0.0 +0,0.072000,0.0,0.0,0.0,0.0,0.0 +0,0.072100,0.0,0.0,0.0,0.0,0.0 +0,0.072200,0.0,0.0,0.0,0.0,0.0 +0,0.072300,0.0,0.0,0.0,0.0,0.0 +0,0.072400,0.0,0.0,0.0,0.0,0.0 +0,0.072500,0.0,0.0,0.0,0.0,0.0 +0,0.072600,0.0,0.0,0.0,0.0,0.0 +0,0.072700,0.0,0.0,0.0,0.0,0.0 +0,0.072800,0.0,0.0,0.0,0.0,0.0 +0,0.072900,0.0,0.0,0.0,0.0,0.0 +0,0.073000,0.0,0.0,0.0,0.0,0.0 +0,0.073100,0.0,0.0,0.0,0.0,0.0 +0,0.073200,0.0,0.0,0.0,0.0,0.0 +0,0.073300,0.0,0.0,0.0,0.0,0.0 +0,0.073400,0.0,0.0,0.0,0.0,0.0 +0,0.073500,0.0,0.0,0.0,0.0,0.0 +0,0.073600,0.0,0.0,0.0,0.0,0.0 +0,0.073700,0.0,0.0,0.0,0.0,0.0 +0,0.073800,0.0,0.0,0.0,0.0,0.0 +0,0.073900,0.0,0.0,0.0,0.0,0.0 +0,0.074000,0.0,0.0,0.0,0.0,0.0 +0,0.074100,0.0,0.0,0.0,0.0,0.0 +0,0.074200,0.0,0.0,0.0,0.0,0.0 +0,0.074300,0.0,0.0,0.0,0.0,0.0 +0,0.074400,0.0,0.0,0.0,0.0,0.0 +0,0.074500,0.0,0.0,0.0,0.0,0.0 +0,0.074600,0.0,0.0,0.0,0.0,0.0 +0,0.074700,0.0,0.0,0.0,0.0,0.0 +0,0.074800,0.0,0.0,0.0,0.0,0.0 +0,0.074900,0.0,0.0,0.0,0.0,0.0 +0,0.075000,0.0,0.0,0.0,0.0,0.0 +0,0.075100,0.0,0.0,0.0,0.0,0.0 +0,0.075200,0.0,0.0,0.0,0.0,0.0 +0,0.075300,0.0,0.0,0.0,0.0,0.0 +0,0.075400,0.0,0.0,0.0,0.0,0.0 +0,0.075500,0.0,0.0,0.0,0.0,0.0 +0,0.075600,0.0,0.0,0.0,0.0,0.0 +0,0.075700,0.0,0.0,0.0,0.0,0.0 +0,0.075800,0.0,0.0,0.0,0.0,0.0 +0,0.075900,0.0,0.0,0.0,0.0,0.0 +0,0.076000,0.0,0.0,0.0,0.0,0.0 +0,0.076100,0.0,0.0,0.0,0.0,0.0 +0,0.076200,0.0,0.0,0.0,0.0,0.0 +0,0.076300,0.0,0.0,0.0,0.0,0.0 +0,0.076400,0.0,0.0,0.0,0.0,0.0 +0,0.076500,0.0,0.0,0.0,0.0,0.0 +0,0.076600,0.0,0.0,0.0,0.0,0.0 +0,0.076700,0.0,0.0,0.0,0.0,0.0 +0,0.076800,0.0,0.0,0.0,0.0,0.0 +0,0.076900,0.0,0.0,0.0,0.0,0.0 +0,0.077000,0.0,0.0,0.0,0.0,0.0 +0,0.077100,0.0,0.0,0.0,0.0,0.0 +0,0.077200,0.0,0.0,0.0,0.0,0.0 +0,0.077300,0.0,0.0,0.0,0.0,0.0 +0,0.077400,0.0,0.0,0.0,0.0,0.0 +0,0.077500,0.0,0.0,0.0,0.0,0.0 +0,0.077600,0.0,0.0,0.0,0.0,0.0 +0,0.077700,0.0,0.0,0.0,0.0,0.0 +0,0.077800,0.0,0.0,0.0,0.0,0.0 +0,0.077900,0.0,0.0,0.0,0.0,0.0 +0,0.078000,0.0,0.0,0.0,0.0,0.0 +0,0.078100,0.0,0.0,0.0,0.0,0.0 +0,0.078200,0.0,0.0,0.0,0.0,0.0 +0,0.078300,0.0,0.0,0.0,0.0,0.0 +0,0.078400,0.0,0.0,0.0,0.0,0.0 +0,0.078500,0.0,0.0,0.0,0.0,0.0 +0,0.078600,0.0,0.0,0.0,0.0,0.0 +0,0.078700,0.0,0.0,0.0,0.0,0.0 +0,0.078800,0.0,0.0,0.0,0.0,0.0 +0,0.078900,0.0,0.0,0.0,0.0,0.0 +0,0.079000,0.0,0.0,0.0,0.0,0.0 +0,0.079100,0.0,0.0,0.0,0.0,0.0 +0,0.079200,0.0,0.0,0.0,0.0,0.0 +0,0.079300,0.0,0.0,0.0,0.0,0.0 +0,0.079400,0.0,0.0,0.0,0.0,0.0 +0,0.079500,0.0,0.0,0.0,0.0,0.0 +0,0.079600,0.0,0.0,0.0,0.0,0.0 +0,0.079700,0.0,0.0,0.0,0.0,0.0 +0,0.079800,0.0,0.0,0.0,0.0,0.0 +0,0.079900,0.0,0.0,0.0,0.0,0.0 +0,0.080000,0.0,0.0,0.0,0.0,0.0 +0,0.080100,0.0,0.0,0.0,0.0,0.0 +1,0.214536,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.080200,0.0,0.0,0.0,0.0,0.0 +0,0.080300,0.0,0.0,0.0,0.0,0.0 +0,0.080400,0.0,0.0,0.0,0.0,0.0 +0,0.080500,0.0,0.0,0.0,0.0,0.0 +0,0.080600,0.0,0.0,0.0,0.0,0.0 +0,0.080700,0.0,0.0,0.0,0.0,0.0 +0,0.080800,0.0,0.0,0.0,0.0,0.0 +0,0.080900,0.0,0.0,0.0,0.0,0.0 +0,0.081000,0.0,0.0,0.0,0.0,0.0 +0,0.081100,0.0,0.0,0.0,0.0,0.0 +0,0.081200,0.0,0.0,0.0,0.0,0.0 +0,0.081300,0.0,0.0,0.0,0.0,0.0 +0,0.081400,0.0,0.0,0.0,0.0,0.0 +0,0.081500,0.0,0.0,0.0,0.0,0.0 +0,0.081600,0.0,0.0,0.0,0.0,0.0 +0,0.081700,0.0,0.0,0.0,0.0,0.0 +0,0.081800,0.0,0.0,0.0,0.0,0.0 +0,0.081900,0.0,0.0,0.0,0.0,0.0 +0,0.082000,0.0,0.0,0.0,0.0,0.0 +0,0.082100,0.0,0.0,0.0,0.0,0.0 +0,0.082200,0.0,0.0,0.0,0.0,0.0 +0,0.082300,0.0,0.0,0.0,0.0,0.0 +0,0.082400,0.0,0.0,0.0,0.0,0.0 +0,0.082500,0.0,0.0,0.0,0.0,0.0 +0,0.082600,0.0,0.0,0.0,0.0,0.0 +0,0.082700,0.0,0.0,0.0,0.0,0.0 +0,0.082800,0.0,0.0,0.0,0.0,0.0 +0,0.082900,0.0,0.0,0.0,0.0,0.0 +0,0.083000,0.0,0.0,0.0,0.0,0.0 +0,0.083100,0.0,0.0,0.0,0.0,0.0 +0,0.083200,0.0,0.0,0.0,0.0,0.0 +0,0.083300,0.0,0.0,0.0,0.0,0.0 +0,0.083400,0.0,0.0,0.0,0.0,0.0 +0,0.083500,0.0,0.0,0.0,0.0,0.0 +0,0.083600,0.0,0.0,0.0,0.0,0.0 +0,0.083700,0.0,0.0,0.0,0.0,0.0 +0,0.083800,0.0,0.0,0.0,0.0,0.0 +0,0.083900,0.0,0.0,0.0,0.0,0.0 +0,0.084000,0.0,0.0,0.0,0.0,0.0 +0,0.084100,0.0,0.0,0.0,0.0,0.0 +0,0.084200,0.0,0.0,0.0,0.0,0.0 +0,0.084300,0.0,0.0,0.0,0.0,0.0 +0,0.084400,0.0,0.0,0.0,0.0,0.0 +0,0.084500,0.0,0.0,0.0,0.0,0.0 +0,0.084600,0.0,0.0,0.0,0.0,0.0 +0,0.084700,0.0,0.0,0.0,0.0,0.0 +0,0.084800,0.0,0.0,0.0,0.0,0.0 +0,0.084900,0.0,0.0,0.0,0.0,0.0 +0,0.085000,0.0,0.0,0.0,0.0,0.0 +0,0.085100,0.0,0.0,0.0,0.0,0.0 +0,0.085200,0.0,0.0,0.0,0.0,0.0 +0,0.085300,0.0,0.0,0.0,0.0,0.0 +0,0.085400,0.0,0.0,0.0,0.0,0.0 +0,0.085500,0.0,0.0,0.0,0.0,0.0 +0,0.085600,0.0,0.0,0.0,0.0,0.0 +0,0.085700,0.0,0.0,0.0,0.0,0.0 +0,0.085800,0.0,0.0,0.0,0.0,0.0 +0,0.085900,0.0,0.0,0.0,0.0,0.0 +0,0.086000,0.0,0.0,0.0,0.0,0.0 +0,0.086100,0.0,0.0,0.0,0.0,0.0 +0,0.086200,0.0,0.0,0.0,0.0,0.0 +0,0.086300,0.0,0.0,0.0,0.0,0.0 +0,0.086400,0.0,0.0,0.0,0.0,0.0 +0,0.086500,0.0,0.0,0.0,0.0,0.0 +0,0.086600,0.0,0.0,0.0,0.0,0.0 +0,0.086700,0.0,0.0,0.0,0.0,0.0 +0,0.086800,0.0,0.0,0.0,0.0,0.0 +0,0.086900,0.0,0.0,0.0,0.0,0.0 +0,0.087000,0.0,0.0,0.0,0.0,0.0 +0,0.087100,0.0,0.0,0.0,0.0,0.0 +0,0.087200,0.0,0.0,0.0,0.0,0.0 +0,0.087300,0.0,0.0,0.0,0.0,0.0 +0,0.087400,0.0,0.0,0.0,0.0,0.0 +0,0.087500,0.0,0.0,0.0,0.0,0.0 +0,0.087600,0.0,0.0,0.0,0.0,0.0 +0,0.087700,0.0,0.0,0.0,0.0,0.0 +0,0.087800,0.0,0.0,0.0,0.0,0.0 +0,0.087900,0.0,0.0,0.0,0.0,0.0 +0,0.088000,0.0,0.0,0.0,0.0,0.0 +0,0.088100,0.0,0.0,0.0,0.0,0.0 +0,0.088200,0.0,0.0,0.0,0.0,0.0 +0,0.088300,0.0,0.0,0.0,0.0,0.0 +0,0.088400,0.0,0.0,0.0,0.0,0.0 +0,0.088500,0.0,0.0,0.0,0.0,0.0 +0,0.088600,0.0,0.0,0.0,0.0,0.0 +0,0.088700,0.0,0.0,0.0,0.0,0.0 +0,0.088800,0.0,0.0,0.0,0.0,0.0 +0,0.088900,0.0,0.0,0.0,0.0,0.0 +0,0.089000,0.0,0.0,0.0,0.0,0.0 +0,0.089100,0.0,0.0,0.0,0.0,0.0 +0,0.089200,0.0,0.0,0.0,0.0,0.0 +0,0.089300,0.0,0.0,0.0,0.0,0.0 +0,0.089400,0.0,0.0,0.0,0.0,0.0 +0,0.089500,0.0,0.0,0.0,0.0,0.0 +0,0.089600,0.0,0.0,0.0,0.0,0.0 +0,0.089700,0.0,0.0,0.0,0.0,0.0 +0,0.089800,0.0,0.0,0.0,0.0,0.0 +0,0.089900,0.0,0.0,0.0,0.0,0.0 +0,0.090000,0.0,0.0,0.0,0.0,0.0 +0,0.090100,0.0,0.0,0.0,0.0,0.0 +0,0.090200,0.0,0.0,0.0,0.0,0.0 +0,0.090300,0.0,0.0,0.0,0.0,0.0 +0,0.090400,0.0,0.0,0.0,0.0,0.0 +0,0.090500,0.0,0.0,0.0,0.0,0.0 +0,0.090600,0.0,0.0,0.0,0.0,0.0 +0,0.090700,0.0,0.0,0.0,0.0,0.0 +0,0.090800,0.0,0.0,0.0,0.0,0.0 +0,0.090900,0.0,0.0,0.0,0.0,0.0 +0,0.091000,0.0,0.0,0.0,0.0,0.0 +0,0.091100,0.0,0.0,0.0,0.0,0.0 +0,0.091200,0.0,0.0,0.0,0.0,0.0 +0,0.091300,0.0,0.0,0.0,0.0,0.0 +0,0.091400,0.0,0.0,0.0,0.0,0.0 +0,0.091500,0.0,0.0,0.0,0.0,0.0 +0,0.091600,0.0,0.0,0.0,0.0,0.0 +0,0.091700,0.0,0.0,0.0,0.0,0.0 +0,0.091800,0.0,0.0,0.0,0.0,0.0 +0,0.091900,0.0,0.0,0.0,0.0,0.0 +0,0.092000,0.0,0.0,0.0,0.0,0.0 +0,0.092100,0.0,0.0,0.0,0.0,0.0 +0,0.092200,0.0,0.0,0.0,0.0,0.0 +0,0.092300,0.0,0.0,0.0,0.0,0.0 +0,0.092400,0.0,0.0,0.0,0.0,0.0 +0,0.092500,0.0,0.0,0.0,0.0,0.0 +0,0.092600,0.0,0.0,0.0,0.0,0.0 +0,0.092700,0.0,0.0,0.0,0.0,0.0 +0,0.092800,0.0,0.0,0.0,0.0,0.0 +0,0.092900,0.0,0.0,0.0,0.0,0.0 +0,0.093000,0.0,0.0,0.0,0.0,0.0 +0,0.093100,0.0,0.0,0.0,0.0,0.0 +0,0.093200,0.0,0.0,0.0,0.0,0.0 +0,0.093300,0.0,0.0,0.0,0.0,0.0 +0,0.093400,0.0,0.0,0.0,0.0,0.0 +0,0.093500,0.0,0.0,0.0,0.0,0.0 +0,0.093600,0.0,0.0,0.0,0.0,0.0 +0,0.093700,0.0,0.0,0.0,0.0,0.0 +0,0.093800,0.0,0.0,0.0,0.0,0.0 +0,0.093900,0.0,0.0,0.0,0.0,0.0 +0,0.094000,0.0,0.0,0.0,0.0,0.0 +0,0.094100,0.0,0.0,0.0,0.0,0.0 +0,0.094200,0.0,0.0,0.0,0.0,0.0 +0,0.094300,0.0,0.0,0.0,0.0,0.0 +0,0.094400,0.0,0.0,0.0,0.0,0.0 +0,0.094500,0.0,0.0,0.0,0.0,0.0 +0,0.094600,0.0,0.0,0.0,0.0,0.0 +0,0.094700,0.0,0.0,0.0,0.0,0.0 +0,0.094800,0.0,0.0,0.0,0.0,0.0 +0,0.094900,0.0,0.0,0.0,0.0,0.0 +0,0.095000,0.0,0.0,0.0,0.0,0.0 +0,0.095100,0.0,0.0,0.0,0.0,0.0 +0,0.095200,0.0,0.0,0.0,0.0,0.0 +0,0.095300,0.0,0.0,0.0,0.0,0.0 +0,0.095400,0.0,0.0,0.0,0.0,0.0 +0,0.095500,0.0,0.0,0.0,0.0,0.0 +0,0.095600,0.0,0.0,0.0,0.0,0.0 +0,0.095700,0.0,0.0,0.0,0.0,0.0 +0,0.095800,0.0,0.0,0.0,0.0,0.0 +0,0.095900,0.0,0.0,0.0,0.0,0.0 +0,0.096000,0.0,0.0,0.0,0.0,0.0 +0,0.096100,0.0,0.0,0.0,0.0,0.0 +0,0.096200,0.0,0.0,0.0,0.0,0.0 +0,0.096300,0.0,0.0,0.0,0.0,0.0 +0,0.096400,0.0,0.0,0.0,0.0,0.0 +0,0.096500,0.0,0.0,0.0,0.0,0.0 +0,0.096600,0.0,0.0,0.0,0.0,0.0 +0,0.096700,0.0,0.0,0.0,0.0,0.0 +0,0.096800,0.0,0.0,0.0,0.0,0.0 +0,0.096900,0.0,0.0,0.0,0.0,0.0 +0,0.097000,0.0,0.0,0.0,0.0,0.0 +0,0.097100,0.0,0.0,0.0,0.0,0.0 +0,0.097200,0.0,0.0,0.0,0.0,0.0 +0,0.097300,0.0,0.0,0.0,0.0,0.0 +0,0.097400,0.0,0.0,0.0,0.0,0.0 +0,0.097500,0.0,0.0,0.0,0.0,0.0 +0,0.097600,0.0,0.0,0.0,0.0,0.0 +0,0.097700,0.0,0.0,0.0,0.0,0.0 +0,0.097800,0.0,0.0,0.0,0.0,0.0 +0,0.097900,0.0,0.0,0.0,0.0,0.0 +0,0.098000,0.0,0.0,0.0,0.0,0.0 +0,0.098100,0.0,0.0,0.0,0.0,0.0 +0,0.098200,0.0,0.0,0.0,0.0,0.0 +0,0.098300,0.0,0.0,0.0,0.0,0.0 +0,0.098400,0.0,0.0,0.0,0.0,0.0 +0,0.098500,0.0,0.0,0.0,0.0,0.0 +0,0.098600,0.0,0.0,0.0,0.0,0.0 +0,0.098700,0.0,0.0,0.0,0.0,0.0 +0,0.098800,0.0,0.0,0.0,0.0,0.0 +0,0.098900,0.0,0.0,0.0,0.0,0.0 +0,0.099000,0.0,0.0,0.0,0.0,0.0 +0,0.099100,0.0,0.0,0.0,0.0,0.0 +0,0.099200,0.0,0.0,0.0,0.0,0.0 +0,0.099300,0.0,0.0,0.0,0.0,0.0 +0,0.099400,0.0,0.0,0.0,0.0,0.0 +0,0.099500,0.0,0.0,0.0,0.0,0.0 +0,0.099600,0.0,0.0,0.0,0.0,0.0 +0,0.099700,0.0,0.0,0.0,0.0,0.0 +0,0.099800,0.0,0.0,0.0,0.0,0.0 +0,0.099900,0.0,0.0,0.0,0.0,0.0 +0,0.100000,0.0,0.0,0.0,0.0,0.0 +0,0.100100,0.0,0.0,0.0,0.0,0.0 +1,0.418544,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.100200,0.0,0.0,0.0,0.0,0.0 +0,0.100300,0.0,0.0,0.0,0.0,0.0 +0,0.100400,0.0,0.0,0.0,0.0,0.0 +0,0.100500,0.0,0.0,0.0,0.0,0.0 +0,0.100600,0.0,0.0,0.0,0.0,0.0 +0,0.100700,0.0,0.0,0.0,0.0,0.0 +0,0.100800,0.0,0.0,0.0,0.0,0.0 +0,0.100900,0.0,0.0,0.0,0.0,0.0 +0,0.101000,0.0,0.0,0.0,0.0,0.0 +0,0.101100,0.0,0.0,0.0,0.0,0.0 +0,0.101200,0.0,0.0,0.0,0.0,0.0 +0,0.101300,0.0,0.0,0.0,0.0,0.0 +0,0.101400,0.0,0.0,0.0,0.0,0.0 +0,0.101500,0.0,0.0,0.0,0.0,0.0 +0,0.101600,0.0,0.0,0.0,0.0,0.0 +0,0.101700,0.0,0.0,0.0,0.0,0.0 +0,0.101800,0.0,0.0,0.0,0.0,0.0 +0,0.101900,0.0,0.0,0.0,0.0,0.0 +0,0.102000,0.0,0.0,0.0,0.0,0.0 +0,0.102100,0.0,0.0,0.0,0.0,0.0 +0,0.102200,0.0,0.0,0.0,0.0,0.0 +0,0.102300,0.0,0.0,0.0,0.0,0.0 +0,0.102400,0.0,0.0,0.0,0.0,0.0 +0,0.102500,0.0,0.0,0.0,0.0,0.0 +0,0.102600,0.0,0.0,0.0,0.0,0.0 +0,0.102700,0.0,0.0,0.0,0.0,0.0 +0,0.102800,0.0,0.0,0.0,0.0,0.0 +0,0.102900,0.0,0.0,0.0,0.0,0.0 +0,0.103000,0.0,0.0,0.0,0.0,0.0 +0,0.103100,0.0,0.0,0.0,0.0,0.0 +0,0.103200,0.0,0.0,0.0,0.0,0.0 +0,0.103300,0.0,0.0,0.0,0.0,0.0 +0,0.103400,0.0,0.0,0.0,0.0,0.0 +0,0.103500,0.0,0.0,0.0,0.0,0.0 +0,0.103600,0.0,0.0,0.0,0.0,0.0 +0,0.103700,0.0,0.0,0.0,0.0,0.0 +0,0.103800,0.0,0.0,0.0,0.0,0.0 +0,0.103900,0.0,0.0,0.0,0.0,0.0 +0,0.104000,0.0,0.0,0.0,0.0,0.0 +0,0.104100,0.0,0.0,0.0,0.0,0.0 +0,0.104200,0.0,0.0,0.0,0.0,0.0 +0,0.104300,0.0,0.0,0.0,0.0,0.0 +0,0.104400,0.0,0.0,0.0,0.0,0.0 +0,0.104500,0.0,0.0,0.0,0.0,0.0 +0,0.104600,0.0,0.0,0.0,0.0,0.0 +0,0.104700,0.0,0.0,0.0,0.0,0.0 +0,0.104800,0.0,0.0,0.0,0.0,0.0 +0,0.104900,0.0,0.0,0.0,0.0,0.0 +0,0.105000,0.0,0.0,0.0,0.0,0.0 +0,0.105100,0.0,0.0,0.0,0.0,0.0 +0,0.105200,0.0,0.0,0.0,0.0,0.0 +0,0.105300,0.0,0.0,0.0,0.0,0.0 +0,0.105400,0.0,0.0,0.0,0.0,0.0 +0,0.105500,0.0,0.0,0.0,0.0,0.0 +0,0.105600,0.0,0.0,0.0,0.0,0.0 +0,0.105700,0.0,0.0,0.0,0.0,0.0 +0,0.105800,0.0,0.0,0.0,0.0,0.0 +0,0.105900,0.0,0.0,0.0,0.0,0.0 +0,0.106000,0.0,0.0,0.0,0.0,0.0 +0,0.106100,0.0,0.0,0.0,0.0,0.0 +0,0.106200,0.0,0.0,0.0,0.0,0.0 +0,0.106300,0.0,0.0,0.0,0.0,0.0 +0,0.106400,0.0,0.0,0.0,0.0,0.0 +0,0.106500,0.0,0.0,0.0,0.0,0.0 +0,0.106600,0.0,0.0,0.0,0.0,0.0 +0,0.106700,0.0,0.0,0.0,0.0,0.0 +0,0.106800,0.0,0.0,0.0,0.0,0.0 +0,0.106900,0.0,0.0,0.0,0.0,0.0 +0,0.107000,0.0,0.0,0.0,0.0,0.0 +0,0.107100,0.0,0.0,0.0,0.0,0.0 +0,0.107200,0.0,0.0,0.0,0.0,0.0 +0,0.107300,0.0,0.0,0.0,0.0,0.0 +0,0.107400,0.0,0.0,0.0,0.0,0.0 +0,0.107500,0.0,0.0,0.0,0.0,0.0 +0,0.107600,0.0,0.0,0.0,0.0,0.0 +0,0.107700,0.0,0.0,0.0,0.0,0.0 +0,0.107800,0.0,0.0,0.0,0.0,0.0 +0,0.107900,0.0,0.0,0.0,0.0,0.0 +0,0.108000,0.0,0.0,0.0,0.0,0.0 +0,0.108100,0.0,0.0,0.0,0.0,0.0 +0,0.108200,0.0,0.0,0.0,0.0,0.0 +0,0.108300,0.0,0.0,0.0,0.0,0.0 +0,0.108400,0.0,0.0,0.0,0.0,0.0 +0,0.108500,0.0,0.0,0.0,0.0,0.0 +0,0.108600,0.0,0.0,0.0,0.0,0.0 +0,0.108700,0.0,0.0,0.0,0.0,0.0 +0,0.108800,0.0,0.0,0.0,0.0,0.0 +0,0.108900,0.0,0.0,0.0,0.0,0.0 +0,0.109000,0.0,0.0,0.0,0.0,0.0 +0,0.109100,0.0,0.0,0.0,0.0,0.0 +0,0.109200,0.0,0.0,0.0,0.0,0.0 +0,0.109300,0.0,0.0,0.0,0.0,0.0 +0,0.109400,0.0,0.0,0.0,0.0,0.0 +0,0.109500,0.0,0.0,0.0,0.0,0.0 +0,0.109600,0.0,0.0,0.0,0.0,0.0 +0,0.109700,0.0,0.0,0.0,0.0,0.0 +0,0.109800,0.0,0.0,0.0,0.0,0.0 +0,0.109900,0.0,0.0,0.0,0.0,0.0 +0,0.110000,0.0,0.0,0.0,0.0,0.0 +0,0.110100,0.0,0.0,0.0,0.0,0.0 +0,0.110200,0.0,0.0,0.0,0.0,0.0 +0,0.110300,0.0,0.0,0.0,0.0,0.0 +0,0.110400,0.0,0.0,0.0,0.0,0.0 +0,0.110500,0.0,0.0,0.0,0.0,0.0 +0,0.110600,0.0,0.0,0.0,0.0,0.0 +0,0.110700,0.0,0.0,0.0,0.0,0.0 +0,0.110800,0.0,0.0,0.0,0.0,0.0 +0,0.110900,0.0,0.0,0.0,0.0,0.0 +0,0.111000,0.0,0.0,0.0,0.0,0.0 +0,0.111100,0.0,0.0,0.0,0.0,0.0 +0,0.111200,0.0,0.0,0.0,0.0,0.0 +0,0.111300,0.0,0.0,0.0,0.0,0.0 +0,0.111400,0.0,0.0,0.0,0.0,0.0 +0,0.111500,0.0,0.0,0.0,0.0,0.0 +0,0.111600,0.0,0.0,0.0,0.0,0.0 +0,0.111700,0.0,0.0,0.0,0.0,0.0 +0,0.111800,0.0,0.0,0.0,0.0,0.0 +0,0.111900,0.0,0.0,0.0,0.0,0.0 +0,0.112000,0.0,0.0,0.0,0.0,0.0 +0,0.112100,0.0,0.0,0.0,0.0,0.0 +0,0.112200,0.0,0.0,0.0,0.0,0.0 +0,0.112300,0.0,0.0,0.0,0.0,0.0 +0,0.112400,0.0,0.0,0.0,0.0,0.0 +0,0.112500,0.0,0.0,0.0,0.0,0.0 +0,0.112600,0.0,0.0,0.0,0.0,0.0 +0,0.112700,0.0,0.0,0.0,0.0,0.0 +0,0.112800,0.0,0.0,0.0,0.0,0.0 +0,0.112900,0.0,0.0,0.0,0.0,0.0 +0,0.113000,0.0,0.0,0.0,0.0,0.0 +0,0.113100,0.0,0.0,0.0,0.0,0.0 +0,0.113200,0.0,0.0,0.0,0.0,0.0 +0,0.113300,0.0,0.0,0.0,0.0,0.0 +0,0.113400,0.0,0.0,0.0,0.0,0.0 +0,0.113500,0.0,0.0,0.0,0.0,0.0 +0,0.113600,0.0,0.0,0.0,0.0,0.0 +0,0.113700,0.0,0.0,0.0,0.0,0.0 +0,0.113800,0.0,0.0,0.0,0.0,0.0 +0,0.113900,0.0,0.0,0.0,0.0,0.0 +0,0.114000,0.0,0.0,0.0,0.0,0.0 +0,0.114100,0.0,0.0,0.0,0.0,0.0 +0,0.114200,0.0,0.0,0.0,0.0,0.0 +0,0.114300,0.0,0.0,0.0,0.0,0.0 +0,0.114400,0.0,0.0,0.0,0.0,0.0 +0,0.114500,0.0,0.0,0.0,0.0,0.0 +0,0.114600,0.0,0.0,0.0,0.0,0.0 +0,0.114700,0.0,0.0,0.0,0.0,0.0 +0,0.114800,0.0,0.0,0.0,0.0,0.0 +0,0.114900,0.0,0.0,0.0,0.0,0.0 +0,0.115000,0.0,0.0,0.0,0.0,0.0 +0,0.115100,0.0,0.0,0.0,0.0,0.0 +0,0.115200,0.0,0.0,0.0,0.0,0.0 +0,0.115300,0.0,0.0,0.0,0.0,0.0 +0,0.115400,0.0,0.0,0.0,0.0,0.0 +0,0.115500,0.0,0.0,0.0,0.0,0.0 +0,0.115600,0.0,0.0,0.0,0.0,0.0 +0,0.115700,0.0,0.0,0.0,0.0,0.0 +0,0.115800,0.0,0.0,0.0,0.0,0.0 +0,0.115900,0.0,0.0,0.0,0.0,0.0 +0,0.116000,0.0,0.0,0.0,0.0,0.0 +0,0.116100,0.0,0.0,0.0,0.0,0.0 +0,0.116200,0.0,0.0,0.0,0.0,0.0 +0,0.116300,0.0,0.0,0.0,0.0,0.0 +0,0.116400,0.0,0.0,0.0,0.0,0.0 +0,0.116500,0.0,0.0,0.0,0.0,0.0 +0,0.116600,0.0,0.0,0.0,0.0,0.0 +0,0.116700,0.0,0.0,0.0,0.0,0.0 +0,0.116800,0.0,0.0,0.0,0.0,0.0 +0,0.116900,0.0,0.0,0.0,0.0,0.0 +0,0.117000,0.0,0.0,0.0,0.0,0.0 +0,0.117100,0.0,0.0,0.0,0.0,0.0 +0,0.117200,0.0,0.0,0.0,0.0,0.0 +0,0.117300,0.0,0.0,0.0,0.0,0.0 +0,0.117400,0.0,0.0,0.0,0.0,0.0 +0,0.117500,0.0,0.0,0.0,0.0,0.0 +0,0.117600,0.0,0.0,0.0,0.0,0.0 +0,0.117700,0.0,0.0,0.0,0.0,0.0 +0,0.117800,0.0,0.0,0.0,0.0,0.0 +0,0.117900,0.0,0.0,0.0,0.0,0.0 +0,0.118000,0.0,0.0,0.0,0.0,0.0 +0,0.118100,0.0,0.0,0.0,0.0,0.0 +0,0.118200,0.0,0.0,0.0,0.0,0.0 +0,0.118300,0.0,0.0,0.0,0.0,0.0 +0,0.118400,0.0,0.0,0.0,0.0,0.0 +0,0.118500,0.0,0.0,0.0,0.0,0.0 +0,0.118600,0.0,0.0,0.0,0.0,0.0 +0,0.118700,0.0,0.0,0.0,0.0,0.0 +0,0.118800,0.0,0.0,0.0,0.0,0.0 +0,0.118900,0.0,0.0,0.0,0.0,0.0 +0,0.119000,0.0,0.0,0.0,0.0,0.0 +0,0.119100,0.0,0.0,0.0,0.0,0.0 +0,0.119200,0.0,0.0,0.0,0.0,0.0 +0,0.119300,0.0,0.0,0.0,0.0,0.0 +0,0.119400,0.0,0.0,0.0,0.0,0.0 +0,0.119500,0.0,0.0,0.0,0.0,0.0 +0,0.119600,0.0,0.0,0.0,0.0,0.0 +0,0.119700,0.0,0.0,0.0,0.0,0.0 +0,0.119800,0.0,0.0,0.0,0.0,0.0 +0,0.119900,0.0,0.0,0.0,0.0,0.0 +0,0.120000,0.0,0.0,0.0,0.0,0.0 +0,0.120100,0.0,0.0,0.0,0.0,0.0 +1,0.722703,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.120200,0.0,0.0,0.0,0.0,0.0 +0,0.120300,0.0,0.0,0.0,0.0,0.0 +0,0.120400,0.0,0.0,0.0,0.0,0.0 +0,0.120500,0.0,0.0,0.0,0.0,0.0 +0,0.120600,0.0,0.0,0.0,0.0,0.0 +0,0.120700,0.0,0.0,0.0,0.0,0.0 +0,0.120800,0.0,0.0,0.0,0.0,0.0 +0,0.120900,0.0,0.0,0.0,0.0,0.0 +0,0.121000,0.0,0.0,0.0,0.0,0.0 +0,0.121100,0.0,0.0,0.0,0.0,0.0 +0,0.121200,0.0,0.0,0.0,0.0,0.0 +0,0.121300,0.0,0.0,0.0,0.0,0.0 +0,0.121400,0.0,0.0,0.0,0.0,0.0 +0,0.121500,0.0,0.0,0.0,0.0,0.0 +0,0.121600,0.0,0.0,0.0,0.0,0.0 +0,0.121700,0.0,0.0,0.0,0.0,0.0 +0,0.121800,0.0,0.0,0.0,0.0,0.0 +0,0.121900,0.0,0.0,0.0,0.0,0.0 +0,0.122000,0.0,0.0,0.0,0.0,0.0 +0,0.122100,0.0,0.0,0.0,0.0,0.0 +0,0.122200,0.0,0.0,0.0,0.0,0.0 +0,0.122300,0.0,0.0,0.0,0.0,0.0 +0,0.122400,0.0,0.0,0.0,0.0,0.0 +0,0.122500,0.0,0.0,0.0,0.0,0.0 +0,0.122600,0.0,0.0,0.0,0.0,0.0 +0,0.122700,0.0,0.0,0.0,0.0,0.0 +0,0.122800,0.0,0.0,0.0,0.0,0.0 +0,0.122900,0.0,0.0,0.0,0.0,0.0 +0,0.123000,0.0,0.0,0.0,0.0,0.0 +0,0.123100,0.0,0.0,0.0,0.0,0.0 +0,0.123200,0.0,0.0,0.0,0.0,0.0 +0,0.123300,0.0,0.0,0.0,0.0,0.0 +0,0.123400,0.0,0.0,0.0,0.0,0.0 +0,0.123500,0.0,0.0,0.0,0.0,0.0 +0,0.123600,0.0,0.0,0.0,0.0,0.0 +0,0.123700,0.0,0.0,0.0,0.0,0.0 +0,0.123800,0.0,0.0,0.0,0.0,0.0 +0,0.123900,0.0,0.0,0.0,0.0,0.0 +0,0.124000,0.0,0.0,0.0,0.0,0.0 +0,0.124100,0.0,0.0,0.0,0.0,0.0 +0,0.124200,0.0,0.0,0.0,0.0,0.0 +0,0.124300,0.0,0.0,0.0,0.0,0.0 +0,0.124400,0.0,0.0,0.0,0.0,0.0 +0,0.124500,0.0,0.0,0.0,0.0,0.0 +0,0.124600,0.0,0.0,0.0,0.0,0.0 +0,0.124700,0.0,0.0,0.0,0.0,0.0 +0,0.124800,0.0,0.0,0.0,0.0,0.0 +0,0.124900,0.0,0.0,0.0,0.0,0.0 +0,0.125000,0.0,0.0,0.0,0.0,0.0 +0,0.125100,0.0,0.0,0.0,0.0,0.0 +0,0.125200,0.0,0.0,0.0,0.0,0.0 +0,0.125300,0.0,0.0,0.0,0.0,0.0 +0,0.125400,0.0,0.0,0.0,0.0,0.0 +0,0.125500,0.0,0.0,0.0,0.0,0.0 +0,0.125600,0.0,0.0,0.0,0.0,0.0 +0,0.125700,0.0,0.0,0.0,0.0,0.0 +0,0.125800,0.0,0.0,0.0,0.0,0.0 +0,0.125900,0.0,0.0,0.0,0.0,0.0 +0,0.126000,0.0,0.0,0.0,0.0,0.0 +0,0.126100,0.0,0.0,0.0,0.0,0.0 +0,0.126200,0.0,0.0,0.0,0.0,0.0 +0,0.126300,0.0,0.0,0.0,0.0,0.0 +0,0.126400,0.0,0.0,0.0,0.0,0.0 +0,0.126500,0.0,0.0,0.0,0.0,0.0 +0,0.126600,0.0,0.0,0.0,0.0,0.0 +0,0.126700,0.0,0.0,0.0,0.0,0.0 +0,0.126800,0.0,0.0,0.0,0.0,0.0 +0,0.126900,0.0,0.0,0.0,0.0,0.0 +0,0.127000,0.0,0.0,0.0,0.0,0.0 +0,0.127100,0.0,0.0,0.0,0.0,0.0 +0,0.127200,0.0,0.0,0.0,0.0,0.0 +0,0.127300,0.0,0.0,0.0,0.0,0.0 +0,0.127400,0.0,0.0,0.0,0.0,0.0 +0,0.127500,0.0,0.0,0.0,0.0,0.0 +0,0.127600,0.0,0.0,0.0,0.0,0.0 +0,0.127700,0.0,0.0,0.0,0.0,0.0 +0,0.127800,0.0,0.0,0.0,0.0,0.0 +0,0.127900,0.0,0.0,0.0,0.0,0.0 +0,0.128000,0.0,0.0,0.0,0.0,0.0 +0,0.128100,0.0,0.0,0.0,0.0,0.0 +0,0.128200,0.0,0.0,0.0,0.0,0.0 +0,0.128300,0.0,0.0,0.0,0.0,0.0 +0,0.128400,0.0,0.0,0.0,0.0,0.0 +0,0.128500,0.0,0.0,0.0,0.0,0.0 +0,0.128600,0.0,0.0,0.0,0.0,0.0 +0,0.128700,0.0,0.0,0.0,0.0,0.0 +0,0.128800,0.0,0.0,0.0,0.0,0.0 +0,0.128900,0.0,0.0,0.0,0.0,0.0 +0,0.129000,0.0,0.0,0.0,0.0,0.0 +0,0.129100,0.0,0.0,0.0,0.0,0.0 +0,0.129200,0.0,0.0,0.0,0.0,0.0 +0,0.129300,0.0,0.0,0.0,0.0,0.0 +0,0.129400,0.0,0.0,0.0,0.0,0.0 +0,0.129500,0.0,0.0,0.0,0.0,0.0 +0,0.129600,0.0,0.0,0.0,0.0,0.0 +0,0.129700,0.0,0.0,0.0,0.0,0.0 +0,0.129800,0.0,0.0,0.0,0.0,0.0 +0,0.129900,0.0,0.0,0.0,0.0,0.0 +0,0.130000,0.0,0.0,0.0,0.0,0.0 +0,0.130100,0.0,0.0,0.0,0.0,0.0 +0,0.130200,0.0,0.0,0.0,0.0,0.0 +0,0.130300,0.0,0.0,0.0,0.0,0.0 +0,0.130400,0.0,0.0,0.0,0.0,0.0 +0,0.130500,0.0,0.0,0.0,0.0,0.0 +0,0.130600,0.0,0.0,0.0,0.0,0.0 +0,0.130700,0.0,0.0,0.0,0.0,0.0 +0,0.130800,0.0,0.0,0.0,0.0,0.0 +0,0.130900,0.0,0.0,0.0,0.0,0.0 +0,0.131000,0.0,0.0,0.0,0.0,0.0 +0,0.131100,0.0,0.0,0.0,0.0,0.0 +0,0.131200,0.0,0.0,0.0,0.0,0.0 +0,0.131300,0.0,0.0,0.0,0.0,0.0 +0,0.131400,0.0,0.0,0.0,0.0,0.0 +0,0.131500,0.0,0.0,0.0,0.0,0.0 +0,0.131600,0.0,0.0,0.0,0.0,0.0 +0,0.131700,0.0,0.0,0.0,0.0,0.0 +0,0.131800,0.0,0.0,0.0,0.0,0.0 +0,0.131900,0.0,0.0,0.0,0.0,0.0 +0,0.132000,0.0,0.0,0.0,0.0,0.0 +0,0.132100,0.0,0.0,0.0,0.0,0.0 +0,0.132200,0.0,0.0,0.0,0.0,0.0 +0,0.132300,0.0,0.0,0.0,0.0,0.0 +0,0.132400,0.0,0.0,0.0,0.0,0.0 +0,0.132500,0.0,0.0,0.0,0.0,0.0 +0,0.132600,0.0,0.0,0.0,0.0,0.0 +0,0.132700,0.0,0.0,0.0,0.0,0.0 +0,0.132800,0.0,0.0,0.0,0.0,0.0 +0,0.132900,0.0,0.0,0.0,0.0,0.0 +0,0.133000,0.0,0.0,0.0,0.0,0.0 +0,0.133100,0.0,0.0,0.0,0.0,0.0 +0,0.133200,0.0,0.0,0.0,0.0,0.0 +0,0.133300,0.0,0.0,0.0,0.0,0.0 +0,0.133400,0.0,0.0,0.0,0.0,0.0 +0,0.133500,0.0,0.0,0.0,0.0,0.0 +0,0.133600,0.0,0.0,0.0,0.0,0.0 +0,0.133700,0.0,0.0,0.0,0.0,0.0 +0,0.133800,0.0,0.0,0.0,0.0,0.0 +0,0.133900,0.0,0.0,0.0,0.0,0.0 +0,0.134000,0.0,0.0,0.0,0.0,0.0 +0,0.134100,0.0,0.0,0.0,0.0,0.0 +0,0.134200,0.0,0.0,0.0,0.0,0.0 +0,0.134300,0.0,0.0,0.0,0.0,0.0 +0,0.134400,0.0,0.0,0.0,0.0,0.0 +0,0.134500,0.0,0.0,0.0,0.0,0.0 +0,0.134600,0.0,0.0,0.0,0.0,0.0 +0,0.134700,0.0,0.0,0.0,0.0,0.0 +0,0.134800,0.0,0.0,0.0,0.0,0.0 +0,0.134900,0.0,0.0,0.0,0.0,0.0 +0,0.135000,0.0,0.0,0.0,0.0,0.0 +0,0.135100,0.0,0.0,0.0,0.0,0.0 +0,0.135200,0.0,0.0,0.0,0.0,0.0 +0,0.135300,0.0,0.0,0.0,0.0,0.0 +0,0.135400,0.0,0.0,0.0,0.0,0.0 +0,0.135500,0.0,0.0,0.0,0.0,0.0 +0,0.135600,0.0,0.0,0.0,0.0,0.0 +0,0.135700,0.0,0.0,0.0,0.0,0.0 +0,0.135800,0.0,0.0,0.0,0.0,0.0 +0,0.135900,0.0,0.0,0.0,0.0,0.0 +0,0.136000,0.0,0.0,0.0,0.0,0.0 +0,0.136100,0.0,0.0,0.0,0.0,0.0 +0,0.136200,0.0,0.0,0.0,0.0,0.0 +0,0.136300,0.0,0.0,0.0,0.0,0.0 +0,0.136400,0.0,0.0,0.0,0.0,0.0 +0,0.136500,0.0,0.0,0.0,0.0,0.0 +0,0.136600,0.0,0.0,0.0,0.0,0.0 +0,0.136700,0.0,0.0,0.0,0.0,0.0 +0,0.136800,0.0,0.0,0.0,0.0,0.0 +0,0.136900,0.0,0.0,0.0,0.0,0.0 +0,0.137000,0.0,0.0,0.0,0.0,0.0 +0,0.137100,0.0,0.0,0.0,0.0,0.0 +0,0.137200,0.0,0.0,0.0,0.0,0.0 +0,0.137300,0.0,0.0,0.0,0.0,0.0 +0,0.137400,0.0,0.0,0.0,0.0,0.0 +0,0.137500,0.0,0.0,0.0,0.0,0.0 +0,0.137600,0.0,0.0,0.0,0.0,0.0 +0,0.137700,0.0,0.0,0.0,0.0,0.0 +0,0.137800,0.0,0.0,0.0,0.0,0.0 +0,0.137900,0.0,0.0,0.0,0.0,0.0 +0,0.138000,0.0,0.0,0.0,0.0,0.0 +0,0.138100,0.0,0.0,0.0,0.0,0.0 +0,0.138200,0.0,0.0,0.0,0.0,0.0 +0,0.138300,0.0,0.0,0.0,0.0,0.0 +0,0.138400,0.0,0.0,0.0,0.0,0.0 +0,0.138500,0.0,0.0,0.0,0.0,0.0 +0,0.138600,0.0,0.0,0.0,0.0,0.0 +0,0.138700,0.0,0.0,0.0,0.0,0.0 +0,0.138800,0.0,0.0,0.0,0.0,0.0 +0,0.138900,0.0,0.0,0.0,0.0,0.0 +0,0.139000,0.0,0.0,0.0,0.0,0.0 +0,0.139100,0.0,0.0,0.0,0.0,0.0 +0,0.139200,0.0,0.0,0.0,0.0,0.0 +0,0.139300,0.0,0.0,0.0,0.0,0.0 +0,0.139400,0.0,0.0,0.0,0.0,0.0 +0,0.139500,0.0,0.0,0.0,0.0,0.0 +0,0.139600,0.0,0.0,0.0,0.0,0.0 +0,0.139700,0.0,0.0,0.0,0.0,0.0 +0,0.139800,0.0,0.0,0.0,0.0,0.0 +0,0.139900,0.0,0.0,0.0,0.0,0.0 +0,0.140000,0.0,0.0,0.0,0.0,0.0 +0,0.140100,0.0,0.0,0.0,0.0,0.0 +1,1.147012,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.140200,0.0,0.0,0.0,0.0,0.0 +0,0.140300,0.0,0.0,0.0,0.0,0.0 +0,0.140400,0.0,0.0,0.0,0.0,0.0 +0,0.140500,0.0,0.0,0.0,0.0,0.0 +0,0.140600,0.0,0.0,0.0,0.0,0.0 +0,0.140700,0.0,0.0,0.0,0.0,0.0 +0,0.140800,0.0,0.0,0.0,0.0,0.0 +0,0.140900,0.0,0.0,0.0,0.0,0.0 +0,0.141000,0.0,0.0,0.0,0.0,0.0 +0,0.141100,0.0,0.0,0.0,0.0,0.0 +0,0.141200,0.0,0.0,0.0,0.0,0.0 +0,0.141300,0.0,0.0,0.0,0.0,0.0 +0,0.141400,0.0,0.0,0.0,0.0,0.0 +0,0.141500,0.0,0.0,0.0,0.0,0.0 +0,0.141600,0.0,0.0,0.0,0.0,0.0 +0,0.141700,0.0,0.0,0.0,0.0,0.0 +0,0.141800,0.0,0.0,0.0,0.0,0.0 +0,0.141900,0.0,0.0,0.0,0.0,0.0 +0,0.142000,0.0,0.0,0.0,0.0,0.0 +0,0.142100,0.0,0.0,0.0,0.0,0.0 +0,0.142200,0.0,0.0,0.0,0.0,0.0 +0,0.142300,0.0,0.0,0.0,0.0,0.0 +0,0.142400,0.0,0.0,0.0,0.0,0.0 +0,0.142500,0.0,0.0,0.0,0.0,0.0 +0,0.142600,0.0,0.0,0.0,0.0,0.0 +0,0.142700,0.0,0.0,0.0,0.0,0.0 +0,0.142800,0.0,0.0,0.0,0.0,0.0 +0,0.142900,0.0,0.0,0.0,0.0,0.0 +0,0.143000,0.0,0.0,0.0,0.0,0.0 +0,0.143100,0.0,0.0,0.0,0.0,0.0 +0,0.143200,0.0,0.0,0.0,0.0,0.0 +0,0.143300,0.0,0.0,0.0,0.0,0.0 +0,0.143400,0.0,0.0,0.0,0.0,0.0 +0,0.143500,0.0,0.0,0.0,0.0,0.0 +0,0.143600,0.0,0.0,0.0,0.0,0.0 +0,0.143700,0.0,0.0,0.0,0.0,0.0 +0,0.143800,0.0,0.0,0.0,0.0,0.0 +0,0.143900,0.0,0.0,0.0,0.0,0.0 +0,0.144000,0.0,0.0,0.0,0.0,0.0 +0,0.144100,0.0,0.0,0.0,0.0,0.0 +0,0.144200,0.0,0.0,0.0,0.0,0.0 +0,0.144300,0.0,0.0,0.0,0.0,0.0 +0,0.144400,0.0,0.0,0.0,0.0,0.0 +0,0.144500,0.0,0.0,0.0,0.0,0.0 +0,0.144600,0.0,0.0,0.0,0.0,0.0 +0,0.144700,0.0,0.0,0.0,0.0,0.0 +0,0.144800,0.0,0.0,0.0,0.0,0.0 +0,0.144900,0.0,0.0,0.0,0.0,0.0 +0,0.145000,0.0,0.0,0.0,0.0,0.0 +0,0.145100,0.0,0.0,0.0,0.0,0.0 +0,0.145200,0.0,0.0,0.0,0.0,0.0 +0,0.145300,0.0,0.0,0.0,0.0,0.0 +0,0.145400,0.0,0.0,0.0,0.0,0.0 +0,0.145500,0.0,0.0,0.0,0.0,0.0 +0,0.145600,0.0,0.0,0.0,0.0,0.0 +0,0.145700,0.0,0.0,0.0,0.0,0.0 +0,0.145800,0.0,0.0,0.0,0.0,0.0 +0,0.145900,0.0,0.0,0.0,0.0,0.0 +0,0.146000,0.0,0.0,0.0,0.0,0.0 +0,0.146100,0.0,0.0,0.0,0.0,0.0 +0,0.146200,0.0,0.0,0.0,0.0,0.0 +0,0.146300,0.0,0.0,0.0,0.0,0.0 +0,0.146400,0.0,0.0,0.0,0.0,0.0 +0,0.146500,0.0,0.0,0.0,0.0,0.0 +0,0.146600,0.0,0.0,0.0,0.0,0.0 +0,0.146700,0.0,0.0,0.0,0.0,0.0 +0,0.146800,0.0,0.0,0.0,0.0,0.0 +0,0.146900,0.0,0.0,0.0,0.0,0.0 +0,0.147000,0.0,0.0,0.0,0.0,0.0 +0,0.147100,0.0,0.0,0.0,0.0,0.0 +0,0.147200,0.0,0.0,0.0,0.0,0.0 +0,0.147300,0.0,0.0,0.0,0.0,0.0 +0,0.147400,0.0,0.0,0.0,0.0,0.0 +0,0.147500,0.0,0.0,0.0,0.0,0.0 +0,0.147600,0.0,0.0,0.0,0.0,0.0 +0,0.147700,0.0,0.0,0.0,0.0,0.0 +0,0.147800,0.0,0.0,0.0,0.0,0.0 +0,0.147900,0.0,0.0,0.0,0.0,0.0 +0,0.148000,0.0,0.0,0.0,0.0,0.0 +0,0.148100,0.0,0.0,0.0,0.0,0.0 +0,0.148200,0.0,0.0,0.0,0.0,0.0 +0,0.148300,0.0,0.0,0.0,0.0,0.0 +0,0.148400,0.0,0.0,0.0,0.0,0.0 +0,0.148500,0.0,0.0,0.0,0.0,0.0 +0,0.148600,0.0,0.0,0.0,0.0,0.0 +0,0.148700,0.0,0.0,0.0,0.0,0.0 +0,0.148800,0.0,0.0,0.0,0.0,0.0 +0,0.148900,0.0,0.0,0.0,0.0,0.0 +0,0.149000,0.0,0.0,0.0,0.0,0.0 +0,0.149100,0.0,0.0,0.0,0.0,0.0 +0,0.149200,0.0,0.0,0.0,0.0,0.0 +0,0.149300,0.0,0.0,0.0,0.0,0.0 +0,0.149400,0.0,0.0,0.0,0.0,0.0 +0,0.149500,0.0,0.0,0.0,0.0,0.0 +0,0.149600,0.0,0.0,0.0,0.0,0.0 +0,0.149700,0.0,0.0,0.0,0.0,0.0 +0,0.149800,0.0,0.0,0.0,0.0,0.0 +0,0.149900,0.0,0.0,0.0,0.0,0.0 +0,0.150000,0.0,0.0,0.0,0.0,0.0 +0,0.150100,0.0,0.0,0.0,0.0,0.0 +0,0.150200,0.0,0.0,0.0,0.0,0.0 +0,0.150300,0.0,0.0,0.0,0.0,0.0 +0,0.150400,0.0,0.0,0.0,0.0,0.0 +0,0.150500,0.0,0.0,0.0,0.0,0.0 +0,0.150600,0.0,0.0,0.0,0.0,0.0 +0,0.150700,0.0,0.0,0.0,0.0,0.0 +0,0.150800,0.0,0.0,0.0,0.0,0.0 +0,0.150900,0.0,0.0,0.0,0.0,0.0 +0,0.151000,0.0,0.0,0.0,0.0,0.0 +0,0.151100,0.0,0.0,0.0,0.0,0.0 +0,0.151200,0.0,0.0,0.0,0.0,0.0 +0,0.151300,0.0,0.0,0.0,0.0,0.0 +0,0.151400,0.0,0.0,0.0,0.0,0.0 +0,0.151500,0.0,0.0,0.0,0.0,0.0 +0,0.151600,0.0,0.0,0.0,0.0,0.0 +0,0.151700,0.0,0.0,0.0,0.0,0.0 +0,0.151800,0.0,0.0,0.0,0.0,0.0 +0,0.151900,0.0,0.0,0.0,0.0,0.0 +0,0.152000,0.0,0.0,0.0,0.0,0.0 +0,0.152100,0.0,0.0,0.0,0.0,0.0 +0,0.152200,0.0,0.0,0.0,0.0,0.0 +0,0.152300,0.0,0.0,0.0,0.0,0.0 +0,0.152400,0.0,0.0,0.0,0.0,0.0 +0,0.152500,0.0,0.0,0.0,0.0,0.0 +0,0.152600,0.0,0.0,0.0,0.0,0.0 +0,0.152700,0.0,0.0,0.0,0.0,0.0 +0,0.152800,0.0,0.0,0.0,0.0,0.0 +0,0.152900,0.0,0.0,0.0,0.0,0.0 +0,0.153000,0.0,0.0,0.0,0.0,0.0 +0,0.153100,0.0,0.0,0.0,0.0,0.0 +0,0.153200,0.0,0.0,0.0,0.0,0.0 +0,0.153300,0.0,0.0,0.0,0.0,0.0 +0,0.153400,0.0,0.0,0.0,0.0,0.0 +0,0.153500,0.0,0.0,0.0,0.0,0.0 +0,0.153600,0.0,0.0,0.0,0.0,0.0 +0,0.153700,0.0,0.0,0.0,0.0,0.0 +0,0.153800,0.0,0.0,0.0,0.0,0.0 +0,0.153900,0.0,0.0,0.0,0.0,0.0 +0,0.154000,0.0,0.0,0.0,0.0,0.0 +0,0.154100,0.0,0.0,0.0,0.0,0.0 +0,0.154200,0.0,0.0,0.0,0.0,0.0 +0,0.154300,0.0,0.0,0.0,0.0,0.0 +0,0.154400,0.0,0.0,0.0,0.0,0.0 +0,0.154500,0.0,0.0,0.0,0.0,0.0 +0,0.154600,0.0,0.0,0.0,0.0,0.0 +0,0.154700,0.0,0.0,0.0,0.0,0.0 +0,0.154800,0.0,0.0,0.0,0.0,0.0 +0,0.154900,0.0,0.0,0.0,0.0,0.0 +0,0.155000,0.0,0.0,0.0,0.0,0.0 +0,0.155100,0.0,0.0,0.0,0.0,0.0 +0,0.155200,0.0,0.0,0.0,0.0,0.0 +0,0.155300,0.0,0.0,0.0,0.0,0.0 +0,0.155400,0.0,0.0,0.0,0.0,0.0 +0,0.155500,0.0,0.0,0.0,0.0,0.0 +0,0.155600,0.0,0.0,0.0,0.0,0.0 +0,0.155700,0.0,0.0,0.0,0.0,0.0 +0,0.155800,0.0,0.0,0.0,0.0,0.0 +0,0.155900,0.0,0.0,0.0,0.0,0.0 +0,0.156000,0.0,0.0,0.0,0.0,0.0 +0,0.156100,0.0,0.0,0.0,0.0,0.0 +0,0.156200,0.0,0.0,0.0,0.0,0.0 +0,0.156300,0.0,0.0,0.0,0.0,0.0 +0,0.156400,0.0,0.0,0.0,0.0,0.0 +0,0.156500,0.0,0.0,0.0,0.0,0.0 +0,0.156600,0.0,0.0,0.0,0.0,0.0 +0,0.156700,0.0,0.0,0.0,0.0,0.0 +0,0.156800,0.0,0.0,0.0,0.0,0.0 +0,0.156900,0.0,0.0,0.0,0.0,0.0 +0,0.157000,0.0,0.0,0.0,0.0,0.0 +0,0.157100,0.0,0.0,0.0,0.0,0.0 +0,0.157200,0.0,0.0,0.0,0.0,0.0 +0,0.157300,0.0,0.0,0.0,0.0,0.0 +0,0.157400,0.0,0.0,0.0,0.0,0.0 +0,0.157500,0.0,0.0,0.0,0.0,0.0 +0,0.157600,0.0,0.0,0.0,0.0,0.0 +0,0.157700,0.0,0.0,0.0,0.0,0.0 +0,0.157800,0.0,0.0,0.0,0.0,0.0 +0,0.157900,0.0,0.0,0.0,0.0,0.0 +0,0.158000,0.0,0.0,0.0,0.0,0.0 +0,0.158100,0.0,0.0,0.0,0.0,0.0 +0,0.158200,0.0,0.0,0.0,0.0,0.0 +0,0.158300,0.0,0.0,0.0,0.0,0.0 +0,0.158400,0.0,0.0,0.0,0.0,0.0 +0,0.158500,0.0,0.0,0.0,0.0,0.0 +0,0.158600,0.0,0.0,0.0,0.0,0.0 +0,0.158700,0.0,0.0,0.0,0.0,0.0 +0,0.158800,0.0,0.0,0.0,0.0,0.0 +0,0.158900,0.0,0.0,0.0,0.0,0.0 +0,0.159000,0.0,0.0,0.0,0.0,0.0 +0,0.159100,0.0,0.0,0.0,0.0,0.0 +0,0.159200,0.0,0.0,0.0,0.0,0.0 +0,0.159300,0.0,0.0,0.0,0.0,0.0 +0,0.159400,0.0,0.0,0.0,0.0,0.0 +0,0.159500,0.0,0.0,0.0,0.0,0.0 +0,0.159600,0.0,0.0,0.0,0.0,0.0 +0,0.159700,0.0,0.0,0.0,0.0,0.0 +0,0.159800,0.0,0.0,0.0,0.0,0.0 +0,0.159900,0.0,0.0,0.0,0.0,0.0 +0,0.160000,0.0,0.0,0.0,0.0,0.0 +0,0.160100,0.0,0.0,0.0,0.0,0.0 +1,1.711471,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.160200,0.0,0.0,0.0,0.0,0.0 +0,0.160300,0.0,0.0,0.0,0.0,0.0 +0,0.160400,0.0,0.0,0.0,0.0,0.0 +0,0.160500,0.0,0.0,0.0,0.0,0.0 +0,0.160600,0.0,0.0,0.0,0.0,0.0 +0,0.160700,0.0,0.0,0.0,0.0,0.0 +0,0.160800,0.0,0.0,0.0,0.0,0.0 +0,0.160900,0.0,0.0,0.0,0.0,0.0 +0,0.161000,0.0,0.0,0.0,0.0,0.0 +0,0.161100,0.0,0.0,0.0,0.0,0.0 +0,0.161200,0.0,0.0,0.0,0.0,0.0 +0,0.161300,0.0,0.0,0.0,0.0,0.0 +0,0.161400,0.0,0.0,0.0,0.0,0.0 +0,0.161500,0.0,0.0,0.0,0.0,0.0 +0,0.161600,0.0,0.0,0.0,0.0,0.0 +0,0.161700,0.0,0.0,0.0,0.0,0.0 +0,0.161800,0.0,0.0,0.0,0.0,0.0 +0,0.161900,0.0,0.0,0.0,0.0,0.0 +0,0.162000,0.0,0.0,0.0,0.0,0.0 +0,0.162100,0.0,0.0,0.0,0.0,0.0 +0,0.162200,0.0,0.0,0.0,0.0,0.0 +0,0.162300,0.0,0.0,0.0,0.0,0.0 +0,0.162400,0.0,0.0,0.0,0.0,0.0 +0,0.162500,0.0,0.0,0.0,0.0,0.0 +0,0.162600,0.0,0.0,0.0,0.0,0.0 +0,0.162700,0.0,0.0,0.0,0.0,0.0 +0,0.162800,0.0,0.0,0.0,0.0,0.0 +0,0.162900,0.0,0.0,0.0,0.0,0.0 +0,0.163000,0.0,0.0,0.0,0.0,0.0 +0,0.163100,0.0,0.0,0.0,0.0,0.0 +0,0.163200,0.0,0.0,0.0,0.0,0.0 +0,0.163300,0.0,0.0,0.0,0.0,0.0 +0,0.163400,0.0,0.0,0.0,0.0,0.0 +0,0.163500,0.0,0.0,0.0,0.0,0.0 +0,0.163600,0.0,0.0,0.0,0.0,0.0 +0,0.163700,0.0,0.0,0.0,0.0,0.0 +0,0.163800,0.0,0.0,0.0,0.0,0.0 +0,0.163900,0.0,0.0,0.0,0.0,0.0 +0,0.164000,0.0,0.0,0.0,0.0,0.0 +0,0.164100,0.0,0.0,0.0,0.0,0.0 +0,0.164200,0.0,0.0,0.0,0.0,0.0 +0,0.164300,0.0,0.0,0.0,0.0,0.0 +0,0.164400,0.0,0.0,0.0,0.0,0.0 +0,0.164500,0.0,0.0,0.0,0.0,0.0 +0,0.164600,0.0,0.0,0.0,0.0,0.0 +0,0.164700,0.0,0.0,0.0,0.0,0.0 +0,0.164800,0.0,0.0,0.0,0.0,0.0 +0,0.164900,0.0,0.0,0.0,0.0,0.0 +0,0.165000,0.0,0.0,0.0,0.0,0.0 +0,0.165100,0.0,0.0,0.0,0.0,0.0 +0,0.165200,0.0,0.0,0.0,0.0,0.0 +0,0.165300,0.0,0.0,0.0,0.0,0.0 +0,0.165400,0.0,0.0,0.0,0.0,0.0 +0,0.165500,0.0,0.0,0.0,0.0,0.0 +0,0.165600,0.0,0.0,0.0,0.0,0.0 +0,0.165700,0.0,0.0,0.0,0.0,0.0 +0,0.165800,0.0,0.0,0.0,0.0,0.0 +0,0.165900,0.0,0.0,0.0,0.0,0.0 +0,0.166000,0.0,0.0,0.0,0.0,0.0 +0,0.166100,0.0,0.0,0.0,0.0,0.0 +0,0.166200,0.0,0.0,0.0,0.0,0.0 +0,0.166300,0.0,0.0,0.0,0.0,0.0 +0,0.166400,0.0,0.0,0.0,0.0,0.0 +0,0.166500,0.0,0.0,0.0,0.0,0.0 +0,0.166600,0.0,0.0,0.0,0.0,0.0 +0,0.166700,0.0,0.0,0.0,0.0,0.0 +0,0.166800,0.0,0.0,0.0,0.0,0.0 +0,0.166900,0.0,0.0,0.0,0.0,0.0 +0,0.167000,0.0,0.0,0.0,0.0,0.0 +0,0.167100,0.0,0.0,0.0,0.0,0.0 +0,0.167200,0.0,0.0,0.0,0.0,0.0 +0,0.167300,0.0,0.0,0.0,0.0,0.0 +0,0.167400,0.0,0.0,0.0,0.0,0.0 +0,0.167500,0.0,0.0,0.0,0.0,0.0 +0,0.167600,0.0,0.0,0.0,0.0,0.0 +0,0.167700,0.0,0.0,0.0,0.0,0.0 +0,0.167800,0.0,0.0,0.0,0.0,0.0 +0,0.167900,0.0,0.0,0.0,0.0,0.0 +0,0.168000,0.0,0.0,0.0,0.0,0.0 +0,0.168100,0.0,0.0,0.0,0.0,0.0 +0,0.168200,0.0,0.0,0.0,0.0,0.0 +0,0.168300,0.0,0.0,0.0,0.0,0.0 +0,0.168400,0.0,0.0,0.0,0.0,0.0 +0,0.168500,0.0,0.0,0.0,0.0,0.0 +0,0.168600,0.0,0.0,0.0,0.0,0.0 +0,0.168700,0.0,0.0,0.0,0.0,0.0 +0,0.168800,0.0,0.0,0.0,0.0,0.0 +0,0.168900,0.0,0.0,0.0,0.0,0.0 +0,0.169000,0.0,0.0,0.0,0.0,0.0 +0,0.169100,0.0,0.0,0.0,0.0,0.0 +0,0.169200,0.0,0.0,0.0,0.0,0.0 +0,0.169300,0.0,0.0,0.0,0.0,0.0 +0,0.169400,0.0,0.0,0.0,0.0,0.0 +0,0.169500,0.0,0.0,0.0,0.0,0.0 +0,0.169600,0.0,0.0,0.0,0.0,0.0 +0,0.169700,0.0,0.0,0.0,0.0,0.0 +0,0.169800,0.0,0.0,0.0,0.0,0.0 +0,0.169900,0.0,0.0,0.0,0.0,0.0 +0,0.170000,0.0,0.0,0.0,0.0,0.0 +0,0.170100,0.0,0.0,0.0,0.0,0.0 +0,0.170200,0.0,0.0,0.0,0.0,0.0 +0,0.170300,0.0,0.0,0.0,0.0,0.0 +0,0.170400,0.0,0.0,0.0,0.0,0.0 +0,0.170500,0.0,0.0,0.0,0.0,0.0 +0,0.170600,0.0,0.0,0.0,0.0,0.0 +0,0.170700,0.0,0.0,0.0,0.0,0.0 +0,0.170800,0.0,0.0,0.0,0.0,0.0 +0,0.170900,0.0,0.0,0.0,0.0,0.0 +0,0.171000,0.0,0.0,0.0,0.0,0.0 +0,0.171100,0.0,0.0,0.0,0.0,0.0 +0,0.171200,0.0,0.0,0.0,0.0,0.0 +0,0.171300,0.0,0.0,0.0,0.0,0.0 +0,0.171400,0.0,0.0,0.0,0.0,0.0 +0,0.171500,0.0,0.0,0.0,0.0,0.0 +0,0.171600,0.0,0.0,0.0,0.0,0.0 +0,0.171700,0.0,0.0,0.0,0.0,0.0 +0,0.171800,0.0,0.0,0.0,0.0,0.0 +0,0.171900,0.0,0.0,0.0,0.0,0.0 +0,0.172000,0.0,0.0,0.0,0.0,0.0 +0,0.172100,0.0,0.0,0.0,0.0,0.0 +0,0.172200,0.0,0.0,0.0,0.0,0.0 +0,0.172300,0.0,0.0,0.0,0.0,0.0 +0,0.172400,0.0,0.0,0.0,0.0,0.0 +0,0.172500,0.0,0.0,0.0,0.0,0.0 +0,0.172600,0.0,0.0,0.0,0.0,0.0 +0,0.172700,0.0,0.0,0.0,0.0,0.0 +0,0.172800,0.0,0.0,0.0,0.0,0.0 +0,0.172900,0.0,0.0,0.0,0.0,0.0 +0,0.173000,0.0,0.0,0.0,0.0,0.0 +0,0.173100,0.0,0.0,0.0,0.0,0.0 +0,0.173200,0.0,0.0,0.0,0.0,0.0 +0,0.173300,0.0,0.0,0.0,0.0,0.0 +0,0.173400,0.0,0.0,0.0,0.0,0.0 +0,0.173500,0.0,0.0,0.0,0.0,0.0 +0,0.173600,0.0,0.0,0.0,0.0,0.0 +0,0.173700,0.0,0.0,0.0,0.0,0.0 +0,0.173800,0.0,0.0,0.0,0.0,0.0 +0,0.173900,0.0,0.0,0.0,0.0,0.0 +0,0.174000,0.0,0.0,0.0,0.0,0.0 +0,0.174100,0.0,0.0,0.0,0.0,0.0 +0,0.174200,0.0,0.0,0.0,0.0,0.0 +0,0.174300,0.0,0.0,0.0,0.0,0.0 +0,0.174400,0.0,0.0,0.0,0.0,0.0 +0,0.174500,0.0,0.0,0.0,0.0,0.0 +0,0.174600,0.0,0.0,0.0,0.0,0.0 +0,0.174700,0.0,0.0,0.0,0.0,0.0 +0,0.174800,0.0,0.0,0.0,0.0,0.0 +0,0.174900,0.0,0.0,0.0,0.0,0.0 +0,0.175000,0.0,0.0,0.0,0.0,0.0 +0,0.175100,0.0,0.0,0.0,0.0,0.0 +0,0.175200,0.0,0.0,0.0,0.0,0.0 +0,0.175300,0.0,0.0,0.0,0.0,0.0 +0,0.175400,0.0,0.0,0.0,0.0,0.0 +0,0.175500,0.0,0.0,0.0,0.0,0.0 +0,0.175600,0.0,0.0,0.0,0.0,0.0 +0,0.175700,0.0,0.0,0.0,0.0,0.0 +0,0.175800,0.0,0.0,0.0,0.0,0.0 +0,0.175900,0.0,0.0,0.0,0.0,0.0 +0,0.176000,0.0,0.0,0.0,0.0,0.0 +0,0.176100,0.0,0.0,0.0,0.0,0.0 +0,0.176200,0.0,0.0,0.0,0.0,0.0 +0,0.176300,0.0,0.0,0.0,0.0,0.0 +0,0.176400,0.0,0.0,0.0,0.0,0.0 +0,0.176500,0.0,0.0,0.0,0.0,0.0 +0,0.176600,0.0,0.0,0.0,0.0,0.0 +0,0.176700,0.0,0.0,0.0,0.0,0.0 +0,0.176800,0.0,0.0,0.0,0.0,0.0 +0,0.176900,0.0,0.0,0.0,0.0,0.0 +0,0.177000,0.0,0.0,0.0,0.0,0.0 +0,0.177100,0.0,0.0,0.0,0.0,0.0 +0,0.177200,0.0,0.0,0.0,0.0,0.0 +0,0.177300,0.0,0.0,0.0,0.0,0.0 +0,0.177400,0.0,0.0,0.0,0.0,0.0 +0,0.177500,0.0,0.0,0.0,0.0,0.0 +0,0.177600,0.0,0.0,0.0,0.0,0.0 +0,0.177700,0.0,0.0,0.0,0.0,0.0 +0,0.177800,0.0,0.0,0.0,0.0,0.0 +0,0.177900,0.0,0.0,0.0,0.0,0.0 +0,0.178000,0.0,0.0,0.0,0.0,0.0 +0,0.178100,0.0,0.0,0.0,0.0,0.0 +0,0.178200,0.0,0.0,0.0,0.0,0.0 +0,0.178300,0.0,0.0,0.0,0.0,0.0 +0,0.178400,0.0,0.0,0.0,0.0,0.0 +0,0.178500,0.0,0.0,0.0,0.0,0.0 +0,0.178600,0.0,0.0,0.0,0.0,0.0 +0,0.178700,0.0,0.0,0.0,0.0,0.0 +0,0.178800,0.0,0.0,0.0,0.0,0.0 +0,0.178900,0.0,0.0,0.0,0.0,0.0 +0,0.179000,0.0,0.0,0.0,0.0,0.0 +0,0.179100,0.0,0.0,0.0,0.0,0.0 +0,0.179200,0.0,0.0,0.0,0.0,0.0 +0,0.179300,0.0,0.0,0.0,0.0,0.0 +0,0.179400,0.0,0.0,0.0,0.0,0.0 +0,0.179500,0.0,0.0,0.0,0.0,0.0 +0,0.179600,0.0,0.0,0.0,0.0,0.0 +0,0.179700,0.0,0.0,0.0,0.0,0.0 +0,0.179800,0.0,0.0,0.0,0.0,0.0 +0,0.179900,0.0,0.0,0.0,0.0,0.0 +0,0.180000,0.0,0.0,0.0,0.0,0.0 +0,0.180100,0.0,0.0,0.0,0.0,0.0 +1,2.436080,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.180200,0.0,0.0,0.0,0.0,0.0 +0,0.180300,0.0,0.0,0.0,0.0,0.0 +0,0.180400,0.0,0.0,0.0,0.0,0.0 +0,0.180500,0.0,0.0,0.0,0.0,0.0 +0,0.180600,0.0,0.0,0.0,0.0,0.0 +0,0.180700,0.0,0.0,0.0,0.0,0.0 +0,0.180800,0.0,0.0,0.0,0.0,0.0 +0,0.180900,0.0,0.0,0.0,0.0,0.0 +0,0.181000,0.0,0.0,0.0,0.0,0.0 +0,0.181100,0.0,0.0,0.0,0.0,0.0 +0,0.181200,0.0,0.0,0.0,0.0,0.0 +0,0.181300,0.0,0.0,0.0,0.0,0.0 +0,0.181400,0.0,0.0,0.0,0.0,0.0 +0,0.181500,0.0,0.0,0.0,0.0,0.0 +0,0.181600,0.0,0.0,0.0,0.0,0.0 +0,0.181700,0.0,0.0,0.0,0.0,0.0 +0,0.181800,0.0,0.0,0.0,0.0,0.0 +0,0.181900,0.0,0.0,0.0,0.0,0.0 +0,0.182000,0.0,0.0,0.0,0.0,0.0 +0,0.182100,0.0,0.0,0.0,0.0,0.0 +0,0.182200,0.0,0.0,0.0,0.0,0.0 +0,0.182300,0.0,0.0,0.0,0.0,0.0 +0,0.182400,0.0,0.0,0.0,0.0,0.0 +0,0.182500,0.0,0.0,0.0,0.0,0.0 +0,0.182600,0.0,0.0,0.0,0.0,0.0 +0,0.182700,0.0,0.0,0.0,0.0,0.0 +0,0.182800,0.0,0.0,0.0,0.0,0.0 +0,0.182900,0.0,0.0,0.0,0.0,0.0 +0,0.183000,0.0,0.0,0.0,0.0,0.0 +0,0.183100,0.0,0.0,0.0,0.0,0.0 +0,0.183200,0.0,0.0,0.0,0.0,0.0 +0,0.183300,0.0,0.0,0.0,0.0,0.0 +0,0.183400,0.0,0.0,0.0,0.0,0.0 +0,0.183500,0.0,0.0,0.0,0.0,0.0 +0,0.183600,0.0,0.0,0.0,0.0,0.0 +0,0.183700,0.0,0.0,0.0,0.0,0.0 +0,0.183800,0.0,0.0,0.0,0.0,0.0 +0,0.183900,0.0,0.0,0.0,0.0,0.0 +0,0.184000,0.0,0.0,0.0,0.0,0.0 +0,0.184100,0.0,0.0,0.0,0.0,0.0 +0,0.184200,0.0,0.0,0.0,0.0,0.0 +0,0.184300,0.0,0.0,0.0,0.0,0.0 +0,0.184400,0.0,0.0,0.0,0.0,0.0 +0,0.184500,0.0,0.0,0.0,0.0,0.0 +0,0.184600,0.0,0.0,0.0,0.0,0.0 +0,0.184700,0.0,0.0,0.0,0.0,0.0 +0,0.184800,0.0,0.0,0.0,0.0,0.0 +0,0.184900,0.0,0.0,0.0,0.0,0.0 +0,0.185000,0.0,0.0,0.0,0.0,0.0 +0,0.185100,0.0,0.0,0.0,0.0,0.0 +0,0.185200,0.0,0.0,0.0,0.0,0.0 +0,0.185300,0.0,0.0,0.0,0.0,0.0 +0,0.185400,0.0,0.0,0.0,0.0,0.0 +0,0.185500,0.0,0.0,0.0,0.0,0.0 +0,0.185600,0.0,0.0,0.0,0.0,0.0 +0,0.185700,0.0,0.0,0.0,0.0,0.0 +0,0.185800,0.0,0.0,0.0,0.0,0.0 +0,0.185900,0.0,0.0,0.0,0.0,0.0 +0,0.186000,0.0,0.0,0.0,0.0,0.0 +0,0.186100,0.0,0.0,0.0,0.0,0.0 +0,0.186200,0.0,0.0,0.0,0.0,0.0 +0,0.186300,0.0,0.0,0.0,0.0,0.0 +0,0.186400,0.0,0.0,0.0,0.0,0.0 +0,0.186500,0.0,0.0,0.0,0.0,0.0 +0,0.186600,0.0,0.0,0.0,0.0,0.0 +0,0.186700,0.0,0.0,0.0,0.0,0.0 +0,0.186800,0.0,0.0,0.0,0.0,0.0 +0,0.186900,0.0,0.0,0.0,0.0,0.0 +0,0.187000,0.0,0.0,0.0,0.0,0.0 +0,0.187100,0.0,0.0,0.0,0.0,0.0 +0,0.187200,0.0,0.0,0.0,0.0,0.0 +0,0.187300,0.0,0.0,0.0,0.0,0.0 +0,0.187400,0.0,0.0,0.0,0.0,0.0 +0,0.187500,0.0,0.0,0.0,0.0,0.0 +0,0.187600,0.0,0.0,0.0,0.0,0.0 +0,0.187700,0.0,0.0,0.0,0.0,0.0 +0,0.187800,0.0,0.0,0.0,0.0,0.0 +0,0.187900,0.0,0.0,0.0,0.0,0.0 +0,0.188000,0.0,0.0,0.0,0.0,0.0 +0,0.188100,0.0,0.0,0.0,0.0,0.0 +0,0.188200,0.0,0.0,0.0,0.0,0.0 +0,0.188300,0.0,0.0,0.0,0.0,0.0 +0,0.188400,0.0,0.0,0.0,0.0,0.0 +0,0.188500,0.0,0.0,0.0,0.0,0.0 +0,0.188600,0.0,0.0,0.0,0.0,0.0 +0,0.188700,0.0,0.0,0.0,0.0,0.0 +0,0.188800,0.0,0.0,0.0,0.0,0.0 +0,0.188900,0.0,0.0,0.0,0.0,0.0 +0,0.189000,0.0,0.0,0.0,0.0,0.0 +0,0.189100,0.0,0.0,0.0,0.0,0.0 +0,0.189200,0.0,0.0,0.0,0.0,0.0 +0,0.189300,0.0,0.0,0.0,0.0,0.0 +0,0.189400,0.0,0.0,0.0,0.0,0.0 +0,0.189500,0.0,0.0,0.0,0.0,0.0 +0,0.189600,0.0,0.0,0.0,0.0,0.0 +0,0.189700,0.0,0.0,0.0,0.0,0.0 +0,0.189800,0.0,0.0,0.0,0.0,0.0 +0,0.189900,0.0,0.0,0.0,0.0,0.0 +0,0.190000,0.0,0.0,0.0,0.0,0.0 +0,0.190100,0.0,0.0,0.0,0.0,0.0 +0,0.190200,0.0,0.0,0.0,0.0,0.0 +0,0.190300,0.0,0.0,0.0,0.0,0.0 +0,0.190400,0.0,0.0,0.0,0.0,0.0 +0,0.190500,0.0,0.0,0.0,0.0,0.0 +0,0.190600,0.0,0.0,0.0,0.0,0.0 +0,0.190700,0.0,0.0,0.0,0.0,0.0 +0,0.190800,0.0,0.0,0.0,0.0,0.0 +0,0.190900,0.0,0.0,0.0,0.0,0.0 +0,0.191000,0.0,0.0,0.0,0.0,0.0 +0,0.191100,0.0,0.0,0.0,0.0,0.0 +0,0.191200,0.0,0.0,0.0,0.0,0.0 +0,0.191300,0.0,0.0,0.0,0.0,0.0 +0,0.191400,0.0,0.0,0.0,0.0,0.0 +0,0.191500,0.0,0.0,0.0,0.0,0.0 +0,0.191600,0.0,0.0,0.0,0.0,0.0 +0,0.191700,0.0,0.0,0.0,0.0,0.0 +0,0.191800,0.0,0.0,0.0,0.0,0.0 +0,0.191900,0.0,0.0,0.0,0.0,0.0 +0,0.192000,0.0,0.0,0.0,0.0,0.0 +0,0.192100,0.0,0.0,0.0,0.0,0.0 +0,0.192200,0.0,0.0,0.0,0.0,0.0 +0,0.192300,0.0,0.0,0.0,0.0,0.0 +0,0.192400,0.0,0.0,0.0,0.0,0.0 +0,0.192500,0.0,0.0,0.0,0.0,0.0 +0,0.192600,0.0,0.0,0.0,0.0,0.0 +0,0.192700,0.0,0.0,0.0,0.0,0.0 +0,0.192800,0.0,0.0,0.0,0.0,0.0 +0,0.192900,0.0,0.0,0.0,0.0,0.0 +0,0.193000,0.0,0.0,0.0,0.0,0.0 +0,0.193100,0.0,0.0,0.0,0.0,0.0 +0,0.193200,0.0,0.0,0.0,0.0,0.0 +0,0.193300,0.0,0.0,0.0,0.0,0.0 +0,0.193400,0.0,0.0,0.0,0.0,0.0 +0,0.193500,0.0,0.0,0.0,0.0,0.0 +0,0.193600,0.0,0.0,0.0,0.0,0.0 +0,0.193700,0.0,0.0,0.0,0.0,0.0 +0,0.193800,0.0,0.0,0.0,0.0,0.0 +0,0.193900,0.0,0.0,0.0,0.0,0.0 +0,0.194000,0.0,0.0,0.0,0.0,0.0 +0,0.194100,0.0,0.0,0.0,0.0,0.0 +0,0.194200,0.0,0.0,0.0,0.0,0.0 +0,0.194300,0.0,0.0,0.0,0.0,0.0 +0,0.194400,0.0,0.0,0.0,0.0,0.0 +0,0.194500,0.0,0.0,0.0,0.0,0.0 +0,0.194600,0.0,0.0,0.0,0.0,0.0 +0,0.194700,0.0,0.0,0.0,0.0,0.0 +0,0.194800,0.0,0.0,0.0,0.0,0.0 +0,0.194900,0.0,0.0,0.0,0.0,0.0 +0,0.195000,0.0,0.0,0.0,0.0,0.0 +0,0.195100,0.0,0.0,0.0,0.0,0.0 +0,0.195200,0.0,0.0,0.0,0.0,0.0 +0,0.195300,0.0,0.0,0.0,0.0,0.0 +0,0.195400,0.0,0.0,0.0,0.0,0.0 +0,0.195500,0.0,0.0,0.0,0.0,0.0 +0,0.195600,0.0,0.0,0.0,0.0,0.0 +0,0.195700,0.0,0.0,0.0,0.0,0.0 +0,0.195800,0.0,0.0,0.0,0.0,0.0 +0,0.195900,0.0,0.0,0.0,0.0,0.0 +0,0.196000,0.0,0.0,0.0,0.0,0.0 +0,0.196100,0.0,0.0,0.0,0.0,0.0 +0,0.196200,0.0,0.0,0.0,0.0,0.0 +0,0.196300,0.0,0.0,0.0,0.0,0.0 +0,0.196400,0.0,0.0,0.0,0.0,0.0 +0,0.196500,0.0,0.0,0.0,0.0,0.0 +0,0.196600,0.0,0.0,0.0,0.0,0.0 +0,0.196700,0.0,0.0,0.0,0.0,0.0 +0,0.196800,0.0,0.0,0.0,0.0,0.0 +0,0.196900,0.0,0.0,0.0,0.0,0.0 +0,0.197000,0.0,0.0,0.0,0.0,0.0 +0,0.197100,0.0,0.0,0.0,0.0,0.0 +0,0.197200,0.0,0.0,0.0,0.0,0.0 +0,0.197300,0.0,0.0,0.0,0.0,0.0 +0,0.197400,0.0,0.0,0.0,0.0,0.0 +0,0.197500,0.0,0.0,0.0,0.0,0.0 +0,0.197600,0.0,0.0,0.0,0.0,0.0 +0,0.197700,0.0,0.0,0.0,0.0,0.0 +0,0.197800,0.0,0.0,0.0,0.0,0.0 +0,0.197900,0.0,0.0,0.0,0.0,0.0 +0,0.198000,0.0,0.0,0.0,0.0,0.0 +0,0.198100,0.0,0.0,0.0,0.0,0.0 +0,0.198200,0.0,0.0,0.0,0.0,0.0 +0,0.198300,0.0,0.0,0.0,0.0,0.0 +0,0.198400,0.0,0.0,0.0,0.0,0.0 +0,0.198500,0.0,0.0,0.0,0.0,0.0 +0,0.198600,0.0,0.0,0.0,0.0,0.0 +0,0.198700,0.0,0.0,0.0,0.0,0.0 +0,0.198800,0.0,0.0,0.0,0.0,0.0 +0,0.198900,0.0,0.0,0.0,0.0,0.0 +0,0.199000,0.0,0.0,0.0,0.0,0.0 +0,0.199100,0.0,0.0,0.0,0.0,0.0 +0,0.199200,0.0,0.0,0.0,0.0,0.0 +0,0.199300,0.0,0.0,0.0,0.0,0.0 +0,0.199400,0.0,0.0,0.0,0.0,0.0 +0,0.199500,0.0,0.0,0.0,0.0,0.0 +0,0.199600,0.0,0.0,0.0,0.0,0.0 +0,0.199700,0.0,0.0,0.0,0.0,0.0 +0,0.199800,0.0,0.0,0.0,0.0,0.0 +0,0.199900,0.0,0.0,0.0,0.0,0.0 +0,0.200000,0.0,0.0,0.0,0.0,0.0 +0,0.200100,0.0,0.0,0.0,0.0,0.0 +1,3.340839,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.200200,0.0,0.0,0.0,0.0,0.0 +0,0.200300,0.0,0.0,0.0,0.0,0.0 +0,0.200400,0.0,0.0,0.0,0.0,0.0 +0,0.200500,0.0,0.0,0.0,0.0,0.0 +0,0.200600,0.0,0.0,0.0,0.0,0.0 +0,0.200700,0.0,0.0,0.0,0.0,0.0 +0,0.200800,0.0,0.0,0.0,0.0,0.0 +0,0.200900,0.0,0.0,0.0,0.0,0.0 +0,0.201000,0.0,0.0,0.0,0.0,0.0 +0,0.201100,0.0,0.0,0.0,0.0,0.0 +0,0.201200,0.0,0.0,0.0,0.0,0.0 +0,0.201300,0.0,0.0,0.0,0.0,0.0 +0,0.201400,0.0,0.0,0.0,0.0,0.0 +0,0.201500,0.0,0.0,0.0,0.0,0.0 +0,0.201600,0.0,0.0,0.0,0.0,0.0 +0,0.201700,0.0,0.0,0.0,0.0,0.0 +0,0.201800,0.0,0.0,0.0,0.0,0.0 +0,0.201900,0.0,0.0,0.0,0.0,0.0 +0,0.202000,0.0,0.0,0.0,0.0,0.0 +0,0.202100,0.0,0.0,0.0,0.0,0.0 +0,0.202200,0.0,0.0,0.0,0.0,0.0 +0,0.202300,0.0,0.0,0.0,0.0,0.0 +0,0.202400,0.0,0.0,0.0,0.0,0.0 +0,0.202500,0.0,0.0,0.0,0.0,0.0 +0,0.202600,0.0,0.0,0.0,0.0,0.0 +0,0.202700,0.0,0.0,0.0,0.0,0.0 +0,0.202800,0.0,0.0,0.0,0.0,0.0 +0,0.202900,0.0,0.0,0.0,0.0,0.0 +0,0.203000,0.0,0.0,0.0,0.0,0.0 +0,0.203100,0.0,0.0,0.0,0.0,0.0 +0,0.203200,0.0,0.0,0.0,0.0,0.0 +0,0.203300,0.0,0.0,0.0,0.0,0.0 +0,0.203400,0.0,0.0,0.0,0.0,0.0 +0,0.203500,0.0,0.0,0.0,0.0,0.0 +0,0.203600,0.0,0.0,0.0,0.0,0.0 +0,0.203700,0.0,0.0,0.0,0.0,0.0 +0,0.203800,0.0,0.0,0.0,0.0,0.0 +0,0.203900,0.0,0.0,0.0,0.0,0.0 +0,0.204000,0.0,0.0,0.0,0.0,0.0 +0,0.204100,0.0,0.0,0.0,0.0,0.0 +0,0.204200,0.0,0.0,0.0,0.0,0.0 +0,0.204300,0.0,0.0,0.0,0.0,0.0 +0,0.204400,0.0,0.0,0.0,0.0,0.0 +0,0.204500,0.0,0.0,0.0,0.0,0.0 +0,0.204600,0.0,0.0,0.0,0.0,0.0 +0,0.204700,0.0,0.0,0.0,0.0,0.0 +0,0.204800,0.0,0.0,0.0,0.0,0.0 +0,0.204900,0.0,0.0,0.0,0.0,0.0 +0,0.205000,0.0,0.0,0.0,0.0,0.0 +0,0.205100,0.0,0.0,0.0,0.0,0.0 +0,0.205200,0.0,0.0,0.0,0.0,0.0 +0,0.205300,0.0,0.0,0.0,0.0,0.0 +0,0.205400,0.0,0.0,0.0,0.0,0.0 +0,0.205500,0.0,0.0,0.0,0.0,0.0 +0,0.205600,0.0,0.0,0.0,0.0,0.0 +0,0.205700,0.0,0.0,0.0,0.0,0.0 +0,0.205800,0.0,0.0,0.0,0.0,0.0 +0,0.205900,0.0,0.0,0.0,0.0,0.0 +0,0.206000,0.0,0.0,0.0,0.0,0.0 +0,0.206100,0.0,0.0,0.0,0.0,0.0 +0,0.206200,0.0,0.0,0.0,0.0,0.0 +0,0.206300,0.0,0.0,0.0,0.0,0.0 +0,0.206400,0.0,0.0,0.0,0.0,0.0 +0,0.206500,0.0,0.0,0.0,0.0,0.0 +0,0.206600,0.0,0.0,0.0,0.0,0.0 +0,0.206700,0.0,0.0,0.0,0.0,0.0 +0,0.206800,0.0,0.0,0.0,0.0,0.0 +0,0.206900,0.0,0.0,0.0,0.0,0.0 +0,0.207000,0.0,0.0,0.0,0.0,0.0 +0,0.207100,0.0,0.0,0.0,0.0,0.0 +0,0.207200,0.0,0.0,0.0,0.0,0.0 +0,0.207300,0.0,0.0,0.0,0.0,0.0 +0,0.207400,0.0,0.0,0.0,0.0,0.0 +0,0.207500,0.0,0.0,0.0,0.0,0.0 +0,0.207600,0.0,0.0,0.0,0.0,0.0 +0,0.207700,0.0,0.0,0.0,0.0,0.0 +0,0.207800,0.0,0.0,0.0,0.0,0.0 +0,0.207900,0.0,0.0,0.0,0.0,0.0 +0,0.208000,0.0,0.0,0.0,0.0,0.0 +0,0.208100,0.0,0.0,0.0,0.0,0.0 +0,0.208200,0.0,0.0,0.0,0.0,0.0 +0,0.208300,0.0,0.0,0.0,0.0,0.0 +0,0.208400,0.0,0.0,0.0,0.0,0.0 +0,0.208500,0.0,0.0,0.0,0.0,0.0 +0,0.208600,0.0,0.0,0.0,0.0,0.0 +0,0.208700,0.0,0.0,0.0,0.0,0.0 +0,0.208800,0.0,0.0,0.0,0.0,0.0 +0,0.208900,0.0,0.0,0.0,0.0,0.0 +0,0.209000,0.0,0.0,0.0,0.0,0.0 +0,0.209100,0.0,0.0,0.0,0.0,0.0 +0,0.209200,0.0,0.0,0.0,0.0,0.0 +0,0.209300,0.0,0.0,0.0,0.0,0.0 +0,0.209400,0.0,0.0,0.0,0.0,0.0 +0,0.209500,0.0,0.0,0.0,0.0,0.0 +0,0.209600,0.0,0.0,0.0,0.0,0.0 +0,0.209700,0.0,0.0,0.0,0.0,0.0 +0,0.209800,0.0,0.0,0.0,0.0,0.0 +0,0.209900,0.0,0.0,0.0,0.0,0.0 +0,0.210000,0.0,0.0,0.0,0.0,0.0 +0,0.210100,0.0,0.0,0.0,0.0,0.0 +0,0.210200,0.0,0.0,0.0,0.0,0.0 +0,0.210300,0.0,0.0,0.0,0.0,0.0 +0,0.210400,0.0,0.0,0.0,0.0,0.0 +0,0.210500,0.0,0.0,0.0,0.0,0.0 +0,0.210600,0.0,0.0,0.0,0.0,0.0 +0,0.210700,0.0,0.0,0.0,0.0,0.0 +0,0.210800,0.0,0.0,0.0,0.0,0.0 +0,0.210900,0.0,0.0,0.0,0.0,0.0 +0,0.211000,0.0,0.0,0.0,0.0,0.0 +0,0.211100,0.0,0.0,0.0,0.0,0.0 +0,0.211200,0.0,0.0,0.0,0.0,0.0 +0,0.211300,0.0,0.0,0.0,0.0,0.0 +0,0.211400,0.0,0.0,0.0,0.0,0.0 +0,0.211500,0.0,0.0,0.0,0.0,0.0 +0,0.211600,0.0,0.0,0.0,0.0,0.0 +0,0.211700,0.0,0.0,0.0,0.0,0.0 +0,0.211800,0.0,0.0,0.0,0.0,0.0 +0,0.211900,0.0,0.0,0.0,0.0,0.0 +0,0.212000,0.0,0.0,0.0,0.0,0.0 +0,0.212100,0.0,0.0,0.0,0.0,0.0 +0,0.212200,0.0,0.0,0.0,0.0,0.0 +0,0.212300,0.0,0.0,0.0,0.0,0.0 +0,0.212400,0.0,0.0,0.0,0.0,0.0 +0,0.212500,0.0,0.0,0.0,0.0,0.0 +0,0.212600,0.0,0.0,0.0,0.0,0.0 +0,0.212700,0.0,0.0,0.0,0.0,0.0 +0,0.212800,0.0,0.0,0.0,0.0,0.0 +0,0.212900,0.0,0.0,0.0,0.0,0.0 +0,0.213000,0.0,0.0,0.0,0.0,0.0 +0,0.213100,0.0,0.0,0.0,0.0,0.0 +0,0.213200,0.0,0.0,0.0,0.0,0.0 +0,0.213300,0.0,0.0,0.0,0.0,0.0 +0,0.213400,0.0,0.0,0.0,0.0,0.0 +0,0.213500,0.0,0.0,0.0,0.0,0.0 +0,0.213600,0.0,0.0,0.0,0.0,0.0 +0,0.213700,0.0,0.0,0.0,0.0,0.0 +0,0.213800,0.0,0.0,0.0,0.0,0.0 +0,0.213900,0.0,0.0,0.0,0.0,0.0 +0,0.214000,0.0,0.0,0.0,0.0,0.0 +0,0.214100,0.0,0.0,0.0,0.0,0.0 +0,0.214200,0.0,0.0,0.0,0.0,0.0 +0,0.214300,0.0,0.0,0.0,0.0,0.0 +0,0.214400,0.0,0.0,0.0,0.0,0.0 +0,0.214500,0.0,0.0,0.0,0.0,0.0 +0,0.214600,0.0,0.0,0.0,0.0,0.0 +0,0.214700,0.0,0.0,0.0,0.0,0.0 +0,0.214800,0.0,0.0,0.0,0.0,0.0 +0,0.214900,0.0,0.0,0.0,0.0,0.0 +0,0.215000,0.0,0.0,0.0,0.0,0.0 +0,0.215100,0.0,0.0,0.0,0.0,0.0 +0,0.215200,0.0,0.0,0.0,0.0,0.0 +0,0.215300,0.0,0.0,0.0,0.0,0.0 +0,0.215400,0.0,0.0,0.0,0.0,0.0 +0,0.215500,0.0,0.0,0.0,0.0,0.0 +0,0.215600,0.0,0.0,0.0,0.0,0.0 +0,0.215700,0.0,0.0,0.0,0.0,0.0 +0,0.215800,0.0,0.0,0.0,0.0,0.0 +0,0.215900,0.0,0.0,0.0,0.0,0.0 +0,0.216000,0.0,0.0,0.0,0.0,0.0 +0,0.216100,0.0,0.0,0.0,0.0,0.0 +0,0.216200,0.0,0.0,0.0,0.0,0.0 +0,0.216300,0.0,0.0,0.0,0.0,0.0 +0,0.216400,0.0,0.0,0.0,0.0,0.0 +0,0.216500,0.0,0.0,0.0,0.0,0.0 +0,0.216600,0.0,0.0,0.0,0.0,0.0 +0,0.216700,0.0,0.0,0.0,0.0,0.0 +0,0.216800,0.0,0.0,0.0,0.0,0.0 +0,0.216900,0.0,0.0,0.0,0.0,0.0 +0,0.217000,0.0,0.0,0.0,0.0,0.0 +0,0.217100,0.0,0.0,0.0,0.0,0.0 +0,0.217200,0.0,0.0,0.0,0.0,0.0 +0,0.217300,0.0,0.0,0.0,0.0,0.0 +0,0.217400,0.0,0.0,0.0,0.0,0.0 +0,0.217500,0.0,0.0,0.0,0.0,0.0 +0,0.217600,0.0,0.0,0.0,0.0,0.0 +0,0.217700,0.0,0.0,0.0,0.0,0.0 +0,0.217800,0.0,0.0,0.0,0.0,0.0 +0,0.217900,0.0,0.0,0.0,0.0,0.0 +0,0.218000,0.0,0.0,0.0,0.0,0.0 +0,0.218100,0.0,0.0,0.0,0.0,0.0 +0,0.218200,0.0,0.0,0.0,0.0,0.0 +0,0.218300,0.0,0.0,0.0,0.0,0.0 +0,0.218400,0.0,0.0,0.0,0.0,0.0 +0,0.218500,0.0,0.0,0.0,0.0,0.0 +0,0.218600,0.0,0.0,0.0,0.0,0.0 +0,0.218700,0.0,0.0,0.0,0.0,0.0 +0,0.218800,0.0,0.0,0.0,0.0,0.0 +0,0.218900,0.0,0.0,0.0,0.0,0.0 +0,0.219000,0.0,0.0,0.0,0.0,0.0 +0,0.219100,0.0,0.0,0.0,0.0,0.0 +0,0.219200,0.0,0.0,0.0,0.0,0.0 +0,0.219300,0.0,0.0,0.0,0.0,0.0 +0,0.219400,0.0,0.0,0.0,0.0,0.0 +0,0.219500,0.0,0.0,0.0,0.0,0.0 +0,0.219600,0.0,0.0,0.0,0.0,0.0 +0,0.219700,0.0,0.0,0.0,0.0,0.0 +0,0.219800,0.0,0.0,0.0,0.0,0.0 +0,0.219900,0.0,0.0,0.0,0.0,0.0 +0,0.220000,0.0,0.0,0.0,0.0,0.0 +0,0.220100,0.0,0.0,0.0,0.0,0.0 +1,4.445748,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.220200,0.0,0.0,0.0,0.0,0.0 +0,0.220300,0.0,0.0,0.0,0.0,0.0 +0,0.220400,0.0,0.0,0.0,0.0,0.0 +0,0.220500,0.0,0.0,0.0,0.0,0.0 +0,0.220600,0.0,0.0,0.0,0.0,0.0 +0,0.220700,0.0,0.0,0.0,0.0,0.0 +0,0.220800,0.0,0.0,0.0,0.0,0.0 +0,0.220900,0.0,0.0,0.0,0.0,0.0 +0,0.221000,0.0,0.0,0.0,0.0,0.0 +0,0.221100,0.0,0.0,0.0,0.0,0.0 +0,0.221200,0.0,0.0,0.0,0.0,0.0 +0,0.221300,0.0,0.0,0.0,0.0,0.0 +0,0.221400,0.0,0.0,0.0,0.0,0.0 +0,0.221500,0.0,0.0,0.0,0.0,0.0 +0,0.221600,0.0,0.0,0.0,0.0,0.0 +0,0.221700,0.0,0.0,0.0,0.0,0.0 +0,0.221800,0.0,0.0,0.0,0.0,0.0 +0,0.221900,0.0,0.0,0.0,0.0,0.0 +0,0.222000,0.0,0.0,0.0,0.0,0.0 +0,0.222100,0.0,0.0,0.0,0.0,0.0 +0,0.222200,0.0,0.0,0.0,0.0,0.0 +0,0.222300,0.0,0.0,0.0,0.0,0.0 +0,0.222400,0.0,0.0,0.0,0.0,0.0 +0,0.222500,0.0,0.0,0.0,0.0,0.0 +0,0.222600,0.0,0.0,0.0,0.0,0.0 +0,0.222700,0.0,0.0,0.0,0.0,0.0 +0,0.222800,0.0,0.0,0.0,0.0,0.0 +0,0.222900,0.0,0.0,0.0,0.0,0.0 +0,0.223000,0.0,0.0,0.0,0.0,0.0 +0,0.223100,0.0,0.0,0.0,0.0,0.0 +0,0.223200,0.0,0.0,0.0,0.0,0.0 +0,0.223300,0.0,0.0,0.0,0.0,0.0 +0,0.223400,0.0,0.0,0.0,0.0,0.0 +0,0.223500,0.0,0.0,0.0,0.0,0.0 +0,0.223600,0.0,0.0,0.0,0.0,0.0 +0,0.223700,0.0,0.0,0.0,0.0,0.0 +0,0.223800,0.0,0.0,0.0,0.0,0.0 +0,0.223900,0.0,0.0,0.0,0.0,0.0 +0,0.224000,0.0,0.0,0.0,0.0,0.0 +0,0.224100,0.0,0.0,0.0,0.0,0.0 +0,0.224200,0.0,0.0,0.0,0.0,0.0 +0,0.224300,0.0,0.0,0.0,0.0,0.0 +0,0.224400,0.0,0.0,0.0,0.0,0.0 +0,0.224500,0.0,0.0,0.0,0.0,0.0 +0,0.224600,0.0,0.0,0.0,0.0,0.0 +0,0.224700,0.0,0.0,0.0,0.0,0.0 +0,0.224800,0.0,0.0,0.0,0.0,0.0 +0,0.224900,0.0,0.0,0.0,0.0,0.0 +0,0.225000,0.0,0.0,0.0,0.0,0.0 +0,0.225100,0.0,0.0,0.0,0.0,0.0 +0,0.225200,0.0,0.0,0.0,0.0,0.0 +0,0.225300,0.0,0.0,0.0,0.0,0.0 +0,0.225400,0.0,0.0,0.0,0.0,0.0 +0,0.225500,0.0,0.0,0.0,0.0,0.0 +0,0.225600,0.0,0.0,0.0,0.0,0.0 +0,0.225700,0.0,0.0,0.0,0.0,0.0 +0,0.225800,0.0,0.0,0.0,0.0,0.0 +0,0.225900,0.0,0.0,0.0,0.0,0.0 +0,0.226000,0.0,0.0,0.0,0.0,0.0 +0,0.226100,0.0,0.0,0.0,0.0,0.0 +0,0.226200,0.0,0.0,0.0,0.0,0.0 +0,0.226300,0.0,0.0,0.0,0.0,0.0 +0,0.226400,0.0,0.0,0.0,0.0,0.0 +0,0.226500,0.0,0.0,0.0,0.0,0.0 +0,0.226600,0.0,0.0,0.0,0.0,0.0 +0,0.226700,0.0,0.0,0.0,0.0,0.0 +0,0.226800,0.0,0.0,0.0,0.0,0.0 +0,0.226900,0.0,0.0,0.0,0.0,0.0 +0,0.227000,0.0,0.0,0.0,0.0,0.0 +0,0.227100,0.0,0.0,0.0,0.0,0.0 +0,0.227200,0.0,0.0,0.0,0.0,0.0 +0,0.227300,0.0,0.0,0.0,0.0,0.0 +0,0.227400,0.0,0.0,0.0,0.0,0.0 +0,0.227500,0.0,0.0,0.0,0.0,0.0 +0,0.227600,0.0,0.0,0.0,0.0,0.0 +0,0.227700,0.0,0.0,0.0,0.0,0.0 +0,0.227800,0.0,0.0,0.0,0.0,0.0 +0,0.227900,0.0,0.0,0.0,0.0,0.0 +0,0.228000,0.0,0.0,0.0,0.0,0.0 +0,0.228100,0.0,0.0,0.0,0.0,0.0 +0,0.228200,0.0,0.0,0.0,0.0,0.0 +0,0.228300,0.0,0.0,0.0,0.0,0.0 +0,0.228400,0.0,0.0,0.0,0.0,0.0 +0,0.228500,0.0,0.0,0.0,0.0,0.0 +0,0.228600,0.0,0.0,0.0,0.0,0.0 +0,0.228700,0.0,0.0,0.0,0.0,0.0 +0,0.228800,0.0,0.0,0.0,0.0,0.0 +0,0.228900,0.0,0.0,0.0,0.0,0.0 +0,0.229000,0.0,0.0,0.0,0.0,0.0 +0,0.229100,0.0,0.0,0.0,0.0,0.0 +0,0.229200,0.0,0.0,0.0,0.0,0.0 +0,0.229300,0.0,0.0,0.0,0.0,0.0 +0,0.229400,0.0,0.0,0.0,0.0,0.0 +0,0.229500,0.0,0.0,0.0,0.0,0.0 +0,0.229600,0.0,0.0,0.0,0.0,0.0 +0,0.229700,0.0,0.0,0.0,0.0,0.0 +0,0.229800,0.0,0.0,0.0,0.0,0.0 +0,0.229900,0.0,0.0,0.0,0.0,0.0 +0,0.230000,0.0,0.0,0.0,0.0,0.0 +0,0.230100,0.0,0.0,0.0,0.0,0.0 +0,0.230200,0.0,0.0,0.0,0.0,0.0 +0,0.230300,0.0,0.0,0.0,0.0,0.0 +0,0.230400,0.0,0.0,0.0,0.0,0.0 +0,0.230500,0.0,0.0,0.0,0.0,0.0 +0,0.230600,0.0,0.0,0.0,0.0,0.0 +0,0.230700,0.0,0.0,0.0,0.0,0.0 +0,0.230800,0.0,0.0,0.0,0.0,0.0 +0,0.230900,0.0,0.0,0.0,0.0,0.0 +0,0.231000,0.0,0.0,0.0,0.0,0.0 +0,0.231100,0.0,0.0,0.0,0.0,0.0 +0,0.231200,0.0,0.0,0.0,0.0,0.0 +0,0.231300,0.0,0.0,0.0,0.0,0.0 +0,0.231400,0.0,0.0,0.0,0.0,0.0 +0,0.231500,0.0,0.0,0.0,0.0,0.0 +0,0.231600,0.0,0.0,0.0,0.0,0.0 +0,0.231700,0.0,0.0,0.0,0.0,0.0 +0,0.231800,0.0,0.0,0.0,0.0,0.0 +0,0.231900,0.0,0.0,0.0,0.0,0.0 +0,0.232000,0.0,0.0,0.0,0.0,0.0 +0,0.232100,0.0,0.0,0.0,0.0,0.0 +0,0.232200,0.0,0.0,0.0,0.0,0.0 +0,0.232300,0.0,0.0,0.0,0.0,0.0 +0,0.232400,0.0,0.0,0.0,0.0,0.0 +0,0.232500,0.0,0.0,0.0,0.0,0.0 +0,0.232600,0.0,0.0,0.0,0.0,0.0 +0,0.232700,0.0,0.0,0.0,0.0,0.0 +0,0.232800,0.0,0.0,0.0,0.0,0.0 +0,0.232900,0.0,0.0,0.0,0.0,0.0 +0,0.233000,0.0,0.0,0.0,0.0,0.0 +0,0.233100,0.0,0.0,0.0,0.0,0.0 +0,0.233200,0.0,0.0,0.0,0.0,0.0 +0,0.233300,0.0,0.0,0.0,0.0,0.0 +0,0.233400,0.0,0.0,0.0,0.0,0.0 +0,0.233500,0.0,0.0,0.0,0.0,0.0 +0,0.233600,0.0,0.0,0.0,0.0,0.0 +0,0.233700,0.0,0.0,0.0,0.0,0.0 +0,0.233800,0.0,0.0,0.0,0.0,0.0 +0,0.233900,0.0,0.0,0.0,0.0,0.0 +0,0.234000,0.0,0.0,0.0,0.0,0.0 +0,0.234100,0.0,0.0,0.0,0.0,0.0 +0,0.234200,0.0,0.0,0.0,0.0,0.0 +0,0.234300,0.0,0.0,0.0,0.0,0.0 +0,0.234400,0.0,0.0,0.0,0.0,0.0 +0,0.234500,0.0,0.0,0.0,0.0,0.0 +0,0.234600,0.0,0.0,0.0,0.0,0.0 +0,0.234700,0.0,0.0,0.0,0.0,0.0 +0,0.234800,0.0,0.0,0.0,0.0,0.0 +0,0.234900,0.0,0.0,0.0,0.0,0.0 +0,0.235000,0.0,0.0,0.0,0.0,0.0 +0,0.235100,0.0,0.0,0.0,0.0,0.0 +0,0.235200,0.0,0.0,0.0,0.0,0.0 +0,0.235300,0.0,0.0,0.0,0.0,0.0 +0,0.235400,0.0,0.0,0.0,0.0,0.0 +0,0.235500,0.0,0.0,0.0,0.0,0.0 +0,0.235600,0.0,0.0,0.0,0.0,0.0 +0,0.235700,0.0,0.0,0.0,0.0,0.0 +0,0.235800,0.0,0.0,0.0,0.0,0.0 +0,0.235900,0.0,0.0,0.0,0.0,0.0 +0,0.236000,0.0,0.0,0.0,0.0,0.0 +0,0.236100,0.0,0.0,0.0,0.0,0.0 +0,0.236200,0.0,0.0,0.0,0.0,0.0 +0,0.236300,0.0,0.0,0.0,0.0,0.0 +0,0.236400,0.0,0.0,0.0,0.0,0.0 +0,0.236500,0.0,0.0,0.0,0.0,0.0 +0,0.236600,0.0,0.0,0.0,0.0,0.0 +0,0.236700,0.0,0.0,0.0,0.0,0.0 +0,0.236800,0.0,0.0,0.0,0.0,0.0 +0,0.236900,0.0,0.0,0.0,0.0,0.0 +0,0.237000,0.0,0.0,0.0,0.0,0.0 +0,0.237100,0.0,0.0,0.0,0.0,0.0 +0,0.237200,0.0,0.0,0.0,0.0,0.0 +0,0.237300,0.0,0.0,0.0,0.0,0.0 +0,0.237400,0.0,0.0,0.0,0.0,0.0 +0,0.237500,0.0,0.0,0.0,0.0,0.0 +0,0.237600,0.0,0.0,0.0,0.0,0.0 +0,0.237700,0.0,0.0,0.0,0.0,0.0 +0,0.237800,0.0,0.0,0.0,0.0,0.0 +0,0.237900,0.0,0.0,0.0,0.0,0.0 +0,0.238000,0.0,0.0,0.0,0.0,0.0 +0,0.238100,0.0,0.0,0.0,0.0,0.0 +0,0.238200,0.0,0.0,0.0,0.0,0.0 +0,0.238300,0.0,0.0,0.0,0.0,0.0 +0,0.238400,0.0,0.0,0.0,0.0,0.0 +0,0.238500,0.0,0.0,0.0,0.0,0.0 +0,0.238600,0.0,0.0,0.0,0.0,0.0 +0,0.238700,0.0,0.0,0.0,0.0,0.0 +0,0.238800,0.0,0.0,0.0,0.0,0.0 +0,0.238900,0.0,0.0,0.0,0.0,0.0 +0,0.239000,0.0,0.0,0.0,0.0,0.0 +0,0.239100,0.0,0.0,0.0,0.0,0.0 +0,0.239200,0.0,0.0,0.0,0.0,0.0 +0,0.239300,0.0,0.0,0.0,0.0,0.0 +0,0.239400,0.0,0.0,0.0,0.0,0.0 +0,0.239500,0.0,0.0,0.0,0.0,0.0 +0,0.239600,0.0,0.0,0.0,0.0,0.0 +0,0.239700,0.0,0.0,0.0,0.0,0.0 +0,0.239800,0.0,0.0,0.0,0.0,0.0 +0,0.239900,0.0,0.0,0.0,0.0,0.0 +0,0.240000,0.0,0.0,0.0,0.0,0.0 +0,0.240100,0.0,0.0,0.0,0.0,0.0 +1,5.770807,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.240200,0.0,0.0,0.0,0.0,0.0 +0,0.240300,0.0,0.0,0.0,0.0,0.0 +0,0.240400,0.0,0.0,0.0,0.0,0.0 +0,0.240500,0.0,0.0,0.0,0.0,0.0 +0,0.240600,0.0,0.0,0.0,0.0,0.0 +0,0.240700,0.0,0.0,0.0,0.0,0.0 +0,0.240800,0.0,0.0,0.0,0.0,0.0 +0,0.240900,0.0,0.0,0.0,0.0,0.0 +0,0.241000,0.0,0.0,0.0,0.0,0.0 +0,0.241100,0.0,0.0,0.0,0.0,0.0 +0,0.241200,0.0,0.0,0.0,0.0,0.0 +0,0.241300,0.0,0.0,0.0,0.0,0.0 +0,0.241400,0.0,0.0,0.0,0.0,0.0 +0,0.241500,0.0,0.0,0.0,0.0,0.0 +0,0.241600,0.0,0.0,0.0,0.0,0.0 +0,0.241700,0.0,0.0,0.0,0.0,0.0 +0,0.241800,0.0,0.0,0.0,0.0,0.0 +0,0.241900,0.0,0.0,0.0,0.0,0.0 +0,0.242000,0.0,0.0,0.0,0.0,0.0 +0,0.242100,0.0,0.0,0.0,0.0,0.0 +0,0.242200,0.0,0.0,0.0,0.0,0.0 +0,0.242300,0.0,0.0,0.0,0.0,0.0 +0,0.242400,0.0,0.0,0.0,0.0,0.0 +0,0.242500,0.0,0.0,0.0,0.0,0.0 +0,0.242600,0.0,0.0,0.0,0.0,0.0 +0,0.242700,0.0,0.0,0.0,0.0,0.0 +0,0.242800,0.0,0.0,0.0,0.0,0.0 +0,0.242900,0.0,0.0,0.0,0.0,0.0 +0,0.243000,0.0,0.0,0.0,0.0,0.0 +0,0.243100,0.0,0.0,0.0,0.0,0.0 +0,0.243200,0.0,0.0,0.0,0.0,0.0 +0,0.243300,0.0,0.0,0.0,0.0,0.0 +0,0.243400,0.0,0.0,0.0,0.0,0.0 +0,0.243500,0.0,0.0,0.0,0.0,0.0 +0,0.243600,0.0,0.0,0.0,0.0,0.0 +0,0.243700,0.0,0.0,0.0,0.0,0.0 +0,0.243800,0.0,0.0,0.0,0.0,0.0 +0,0.243900,0.0,0.0,0.0,0.0,0.0 +0,0.244000,0.0,0.0,0.0,0.0,0.0 +0,0.244100,0.0,0.0,0.0,0.0,0.0 +0,0.244200,0.0,0.0,0.0,0.0,0.0 +0,0.244300,0.0,0.0,0.0,0.0,0.0 +0,0.244400,0.0,0.0,0.0,0.0,0.0 +0,0.244500,0.0,0.0,0.0,0.0,0.0 +0,0.244600,0.0,0.0,0.0,0.0,0.0 +0,0.244700,0.0,0.0,0.0,0.0,0.0 +0,0.244800,0.0,0.0,0.0,0.0,0.0 +0,0.244900,0.0,0.0,0.0,0.0,0.0 +0,0.245000,0.0,0.0,0.0,0.0,0.0 +0,0.245100,0.0,0.0,0.0,0.0,0.0 +0,0.245200,0.0,0.0,0.0,0.0,0.0 +0,0.245300,0.0,0.0,0.0,0.0,0.0 +0,0.245400,0.0,0.0,0.0,0.0,0.0 +0,0.245500,0.0,0.0,0.0,0.0,0.0 +0,0.245600,0.0,0.0,0.0,0.0,0.0 +0,0.245700,0.0,0.0,0.0,0.0,0.0 +0,0.245800,0.0,0.0,0.0,0.0,0.0 +0,0.245900,0.0,0.0,0.0,0.0,0.0 +0,0.246000,0.0,0.0,0.0,0.0,0.0 +0,0.246100,0.0,0.0,0.0,0.0,0.0 +0,0.246200,0.0,0.0,0.0,0.0,0.0 +0,0.246300,0.0,0.0,0.0,0.0,0.0 +0,0.246400,0.0,0.0,0.0,0.0,0.0 +0,0.246500,0.0,0.0,0.0,0.0,0.0 +0,0.246600,0.0,0.0,0.0,0.0,0.0 +0,0.246700,0.0,0.0,0.0,0.0,0.0 +0,0.246800,0.0,0.0,0.0,0.0,0.0 +0,0.246900,0.0,0.0,0.0,0.0,0.0 +0,0.247000,0.0,0.0,0.0,0.0,0.0 +0,0.247100,0.0,0.0,0.0,0.0,0.0 +0,0.247200,0.0,0.0,0.0,0.0,0.0 +0,0.247300,0.0,0.0,0.0,0.0,0.0 +0,0.247400,0.0,0.0,0.0,0.0,0.0 +0,0.247500,0.0,0.0,0.0,0.0,0.0 +0,0.247600,0.0,0.0,0.0,0.0,0.0 +0,0.247700,0.0,0.0,0.0,0.0,0.0 +0,0.247800,0.0,0.0,0.0,0.0,0.0 +0,0.247900,0.0,0.0,0.0,0.0,0.0 +0,0.248000,0.0,0.0,0.0,0.0,0.0 +0,0.248100,0.0,0.0,0.0,0.0,0.0 +0,0.248200,0.0,0.0,0.0,0.0,0.0 +0,0.248300,0.0,0.0,0.0,0.0,0.0 +0,0.248400,0.0,0.0,0.0,0.0,0.0 +0,0.248500,0.0,0.0,0.0,0.0,0.0 +0,0.248600,0.0,0.0,0.0,0.0,0.0 +0,0.248700,0.0,0.0,0.0,0.0,0.0 +0,0.248800,0.0,0.0,0.0,0.0,0.0 +0,0.248900,0.0,0.0,0.0,0.0,0.0 +0,0.249000,0.0,0.0,0.0,0.0,0.0 +0,0.249100,0.0,0.0,0.0,0.0,0.0 +0,0.249200,0.0,0.0,0.0,0.0,0.0 +0,0.249300,0.0,0.0,0.0,0.0,0.0 +0,0.249400,0.0,0.0,0.0,0.0,0.0 +0,0.249500,0.0,0.0,0.0,0.0,0.0 +0,0.249600,0.0,0.0,0.0,0.0,0.0 +0,0.249700,0.0,0.0,0.0,0.0,0.0 +0,0.249800,0.0,0.0,0.0,0.0,0.0 +0,0.249900,0.0,0.0,0.0,0.0,0.0 +0,0.250000,0.0,0.0,0.0,0.0,0.0 +0,0.250100,0.0,0.0,0.0,0.0,0.0 +0,0.250200,0.0,0.0,0.0,0.0,0.0 +0,0.250300,0.0,0.0,0.0,0.0,0.0 +0,0.250400,0.0,0.0,0.0,0.0,0.0 +0,0.250500,0.0,0.0,0.0,0.0,0.0 +0,0.250600,0.0,0.0,0.0,0.0,0.0 +0,0.250700,0.0,0.0,0.0,0.0,0.0 +0,0.250800,0.0,0.0,0.0,0.0,0.0 +0,0.250900,0.0,0.0,0.0,0.0,0.0 +0,0.251000,0.0,0.0,0.0,0.0,0.0 +0,0.251100,0.0,0.0,0.0,0.0,0.0 +0,0.251200,0.0,0.0,0.0,0.0,0.0 +0,0.251300,0.0,0.0,0.0,0.0,0.0 +0,0.251400,0.0,0.0,0.0,0.0,0.0 +0,0.251500,0.0,0.0,0.0,0.0,0.0 +0,0.251600,0.0,0.0,0.0,0.0,0.0 +0,0.251700,0.0,0.0,0.0,0.0,0.0 +0,0.251800,0.0,0.0,0.0,0.0,0.0 +0,0.251900,0.0,0.0,0.0,0.0,0.0 +0,0.252000,0.0,0.0,0.0,0.0,0.0 +0,0.252100,0.0,0.0,0.0,0.0,0.0 +0,0.252200,0.0,0.0,0.0,0.0,0.0 +0,0.252300,0.0,0.0,0.0,0.0,0.0 +0,0.252400,0.0,0.0,0.0,0.0,0.0 +0,0.252500,0.0,0.0,0.0,0.0,0.0 +0,0.252600,0.0,0.0,0.0,0.0,0.0 +0,0.252700,0.0,0.0,0.0,0.0,0.0 +0,0.252800,0.0,0.0,0.0,0.0,0.0 +0,0.252900,0.0,0.0,0.0,0.0,0.0 +0,0.253000,0.0,0.0,0.0,0.0,0.0 +0,0.253100,0.0,0.0,0.0,0.0,0.0 +0,0.253200,0.0,0.0,0.0,0.0,0.0 +0,0.253300,0.0,0.0,0.0,0.0,0.0 +0,0.253400,0.0,0.0,0.0,0.0,0.0 +0,0.253500,0.0,0.0,0.0,0.0,0.0 +0,0.253600,0.0,0.0,0.0,0.0,0.0 +0,0.253700,0.0,0.0,0.0,0.0,0.0 +0,0.253800,0.0,0.0,0.0,0.0,0.0 +0,0.253900,0.0,0.0,0.0,0.0,0.0 +0,0.254000,0.0,0.0,0.0,0.0,0.0 +0,0.254100,0.0,0.0,0.0,0.0,0.0 +0,0.254200,0.0,0.0,0.0,0.0,0.0 +0,0.254300,0.0,0.0,0.0,0.0,0.0 +0,0.254400,0.0,0.0,0.0,0.0,0.0 +0,0.254500,0.0,0.0,0.0,0.0,0.0 +0,0.254600,0.0,0.0,0.0,0.0,0.0 +0,0.254700,0.0,0.0,0.0,0.0,0.0 +0,0.254800,0.0,0.0,0.0,0.0,0.0 +0,0.254900,0.0,0.0,0.0,0.0,0.0 +0,0.255000,0.0,0.0,0.0,0.0,0.0 +0,0.255100,0.0,0.0,0.0,0.0,0.0 +0,0.255200,0.0,0.0,0.0,0.0,0.0 +0,0.255300,0.0,0.0,0.0,0.0,0.0 +0,0.255400,0.0,0.0,0.0,0.0,0.0 +0,0.255500,0.0,0.0,0.0,0.0,0.0 +0,0.255600,0.0,0.0,0.0,0.0,0.0 +0,0.255700,0.0,0.0,0.0,0.0,0.0 +0,0.255800,0.0,0.0,0.0,0.0,0.0 +0,0.255900,0.0,0.0,0.0,0.0,0.0 +0,0.256000,0.0,0.0,0.0,0.0,0.0 +0,0.256100,0.0,0.0,0.0,0.0,0.0 +0,0.256200,0.0,0.0,0.0,0.0,0.0 +0,0.256300,0.0,0.0,0.0,0.0,0.0 +0,0.256400,0.0,0.0,0.0,0.0,0.0 +0,0.256500,0.0,0.0,0.0,0.0,0.0 +0,0.256600,0.0,0.0,0.0,0.0,0.0 +0,0.256700,0.0,0.0,0.0,0.0,0.0 +0,0.256800,0.0,0.0,0.0,0.0,0.0 +0,0.256900,0.0,0.0,0.0,0.0,0.0 +0,0.257000,0.0,0.0,0.0,0.0,0.0 +0,0.257100,0.0,0.0,0.0,0.0,0.0 +0,0.257200,0.0,0.0,0.0,0.0,0.0 +0,0.257300,0.0,0.0,0.0,0.0,0.0 +0,0.257400,0.0,0.0,0.0,0.0,0.0 +0,0.257500,0.0,0.0,0.0,0.0,0.0 +0,0.257600,0.0,0.0,0.0,0.0,0.0 +0,0.257700,0.0,0.0,0.0,0.0,0.0 +0,0.257800,0.0,0.0,0.0,0.0,0.0 +0,0.257900,0.0,0.0,0.0,0.0,0.0 +0,0.258000,0.0,0.0,0.0,0.0,0.0 +0,0.258100,0.0,0.0,0.0,0.0,0.0 +0,0.258200,0.0,0.0,0.0,0.0,0.0 +0,0.258300,0.0,0.0,0.0,0.0,0.0 +0,0.258400,0.0,0.0,0.0,0.0,0.0 +0,0.258500,0.0,0.0,0.0,0.0,0.0 +0,0.258600,0.0,0.0,0.0,0.0,0.0 +0,0.258700,0.0,0.0,0.0,0.0,0.0 +0,0.258800,0.0,0.0,0.0,0.0,0.0 +0,0.258900,0.0,0.0,0.0,0.0,0.0 +0,0.259000,0.0,0.0,0.0,0.0,0.0 +0,0.259100,0.0,0.0,0.0,0.0,0.0 +0,0.259200,0.0,0.0,0.0,0.0,0.0 +0,0.259300,0.0,0.0,0.0,0.0,0.0 +0,0.259400,0.0,0.0,0.0,0.0,0.0 +0,0.259500,0.0,0.0,0.0,0.0,0.0 +0,0.259600,0.0,0.0,0.0,0.0,0.0 +0,0.259700,0.0,0.0,0.0,0.0,0.0 +0,0.259800,0.0,0.0,0.0,0.0,0.0 +0,0.259900,0.0,0.0,0.0,0.0,0.0 +0,0.260000,0.0,0.0,0.0,0.0,0.0 +0,0.260100,0.0,0.0,0.0,0.0,0.0 +1,7.336015,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.260200,0.0,0.0,0.0,0.0,0.0 +0,0.260300,0.0,0.0,0.0,0.0,0.0 +0,0.260400,0.0,0.0,0.0,0.0,0.0 +0,0.260500,0.0,0.0,0.0,0.0,0.0 +0,0.260600,0.0,0.0,0.0,0.0,0.0 +0,0.260700,0.0,0.0,0.0,0.0,0.0 +0,0.260800,0.0,0.0,0.0,0.0,0.0 +0,0.260900,0.0,0.0,0.0,0.0,0.0 +0,0.261000,0.0,0.0,0.0,0.0,0.0 +0,0.261100,0.0,0.0,0.0,0.0,0.0 +0,0.261200,0.0,0.0,0.0,0.0,0.0 +0,0.261300,0.0,0.0,0.0,0.0,0.0 +0,0.261400,0.0,0.0,0.0,0.0,0.0 +0,0.261500,0.0,0.0,0.0,0.0,0.0 +0,0.261600,0.0,0.0,0.0,0.0,0.0 +0,0.261700,0.0,0.0,0.0,0.0,0.0 +0,0.261800,0.0,0.0,0.0,0.0,0.0 +0,0.261900,0.0,0.0,0.0,0.0,0.0 +0,0.262000,0.0,0.0,0.0,0.0,0.0 +0,0.262100,0.0,0.0,0.0,0.0,0.0 +0,0.262200,0.0,0.0,0.0,0.0,0.0 +0,0.262300,0.0,0.0,0.0,0.0,0.0 +0,0.262400,0.0,0.0,0.0,0.0,0.0 +0,0.262500,0.0,0.0,0.0,0.0,0.0 +0,0.262600,0.0,0.0,0.0,0.0,0.0 +0,0.262700,0.0,0.0,0.0,0.0,0.0 +0,0.262800,0.0,0.0,0.0,0.0,0.0 +0,0.262900,0.0,0.0,0.0,0.0,0.0 +0,0.263000,0.0,0.0,0.0,0.0,0.0 +0,0.263100,0.0,0.0,0.0,0.0,0.0 +0,0.263200,0.0,0.0,0.0,0.0,0.0 +0,0.263300,0.0,0.0,0.0,0.0,0.0 +0,0.263400,0.0,0.0,0.0,0.0,0.0 +0,0.263500,0.0,0.0,0.0,0.0,0.0 +0,0.263600,0.0,0.0,0.0,0.0,0.0 +0,0.263700,0.0,0.0,0.0,0.0,0.0 +0,0.263800,0.0,0.0,0.0,0.0,0.0 +0,0.263900,0.0,0.0,0.0,0.0,0.0 +0,0.264000,0.0,0.0,0.0,0.0,0.0 +0,0.264100,0.0,0.0,0.0,0.0,0.0 +0,0.264200,0.0,0.0,0.0,0.0,0.0 +0,0.264300,0.0,0.0,0.0,0.0,0.0 +0,0.264400,0.0,0.0,0.0,0.0,0.0 +0,0.264500,0.0,0.0,0.0,0.0,0.0 +0,0.264600,0.0,0.0,0.0,0.0,0.0 +0,0.264700,0.0,0.0,0.0,0.0,0.0 +0,0.264800,0.0,0.0,0.0,0.0,0.0 +0,0.264900,0.0,0.0,0.0,0.0,0.0 +0,0.265000,0.0,0.0,0.0,0.0,0.0 +0,0.265100,0.0,0.0,0.0,0.0,0.0 +0,0.265200,0.0,0.0,0.0,0.0,0.0 +0,0.265300,0.0,0.0,0.0,0.0,0.0 +0,0.265400,0.0,0.0,0.0,0.0,0.0 +0,0.265500,0.0,0.0,0.0,0.0,0.0 +0,0.265600,0.0,0.0,0.0,0.0,0.0 +0,0.265700,0.0,0.0,0.0,0.0,0.0 +0,0.265800,0.0,0.0,0.0,0.0,0.0 +0,0.265900,0.0,0.0,0.0,0.0,0.0 +0,0.266000,0.0,0.0,0.0,0.0,0.0 +0,0.266100,0.0,0.0,0.0,0.0,0.0 +0,0.266200,0.0,0.0,0.0,0.0,0.0 +0,0.266300,0.0,0.0,0.0,0.0,0.0 +0,0.266400,0.0,0.0,0.0,0.0,0.0 +0,0.266500,0.0,0.0,0.0,0.0,0.0 +0,0.266600,0.0,0.0,0.0,0.0,0.0 +0,0.266700,0.0,0.0,0.0,0.0,0.0 +0,0.266800,0.0,0.0,0.0,0.0,0.0 +0,0.266900,0.0,0.0,0.0,0.0,0.0 +0,0.267000,0.0,0.0,0.0,0.0,0.0 +0,0.267100,0.0,0.0,0.0,0.0,0.0 +0,0.267200,0.0,0.0,0.0,0.0,0.0 +0,0.267300,0.0,0.0,0.0,0.0,0.0 +0,0.267400,0.0,0.0,0.0,0.0,0.0 +0,0.267500,0.0,0.0,0.0,0.0,0.0 +0,0.267600,0.0,0.0,0.0,0.0,0.0 +0,0.267700,0.0,0.0,0.0,0.0,0.0 +0,0.267800,0.0,0.0,0.0,0.0,0.0 +0,0.267900,0.0,0.0,0.0,0.0,0.0 +0,0.268000,0.0,0.0,0.0,0.0,0.0 +0,0.268100,0.0,0.0,0.0,0.0,0.0 +0,0.268200,0.0,0.0,0.0,0.0,0.0 +0,0.268300,0.0,0.0,0.0,0.0,0.0 +0,0.268400,0.0,0.0,0.0,0.0,0.0 +0,0.268500,0.0,0.0,0.0,0.0,0.0 +0,0.268600,0.0,0.0,0.0,0.0,0.0 +0,0.268700,0.0,0.0,0.0,0.0,0.0 +0,0.268800,0.0,0.0,0.0,0.0,0.0 +0,0.268900,0.0,0.0,0.0,0.0,0.0 +0,0.269000,0.0,0.0,0.0,0.0,0.0 +0,0.269100,0.0,0.0,0.0,0.0,0.0 +0,0.269200,0.0,0.0,0.0,0.0,0.0 +0,0.269300,0.0,0.0,0.0,0.0,0.0 +0,0.269400,0.0,0.0,0.0,0.0,0.0 +0,0.269500,0.0,0.0,0.0,0.0,0.0 +0,0.269600,0.0,0.0,0.0,0.0,0.0 +0,0.269700,0.0,0.0,0.0,0.0,0.0 +0,0.269800,0.0,0.0,0.0,0.0,0.0 +0,0.269900,0.0,0.0,0.0,0.0,0.0 +0,0.270000,0.0,0.0,0.0,0.0,0.0 +0,0.270100,0.0,0.0,0.0,0.0,0.0 +0,0.270200,0.0,0.0,0.0,0.0,0.0 +0,0.270300,0.0,0.0,0.0,0.0,0.0 +0,0.270400,0.0,0.0,0.0,0.0,0.0 +0,0.270500,0.0,0.0,0.0,0.0,0.0 +0,0.270600,0.0,0.0,0.0,0.0,0.0 +0,0.270700,0.0,0.0,0.0,0.0,0.0 +0,0.270800,0.0,0.0,0.0,0.0,0.0 +0,0.270900,0.0,0.0,0.0,0.0,0.0 +0,0.271000,0.0,0.0,0.0,0.0,0.0 +0,0.271100,0.0,0.0,0.0,0.0,0.0 +0,0.271200,0.0,0.0,0.0,0.0,0.0 +0,0.271300,0.0,0.0,0.0,0.0,0.0 +0,0.271400,0.0,0.0,0.0,0.0,0.0 +0,0.271500,0.0,0.0,0.0,0.0,0.0 +0,0.271600,0.0,0.0,0.0,0.0,0.0 +0,0.271700,0.0,0.0,0.0,0.0,0.0 +0,0.271800,0.0,0.0,0.0,0.0,0.0 +0,0.271900,0.0,0.0,0.0,0.0,0.0 +0,0.272000,0.0,0.0,0.0,0.0,0.0 +0,0.272100,0.0,0.0,0.0,0.0,0.0 +0,0.272200,0.0,0.0,0.0,0.0,0.0 +0,0.272300,0.0,0.0,0.0,0.0,0.0 +0,0.272400,0.0,0.0,0.0,0.0,0.0 +0,0.272500,0.0,0.0,0.0,0.0,0.0 +0,0.272600,0.0,0.0,0.0,0.0,0.0 +0,0.272700,0.0,0.0,0.0,0.0,0.0 +0,0.272800,0.0,0.0,0.0,0.0,0.0 +0,0.272900,0.0,0.0,0.0,0.0,0.0 +0,0.273000,0.0,0.0,0.0,0.0,0.0 +0,0.273100,0.0,0.0,0.0,0.0,0.0 +0,0.273200,0.0,0.0,0.0,0.0,0.0 +0,0.273300,0.0,0.0,0.0,0.0,0.0 +0,0.273400,0.0,0.0,0.0,0.0,0.0 +0,0.273500,0.0,0.0,0.0,0.0,0.0 +0,0.273600,0.0,0.0,0.0,0.0,0.0 +0,0.273700,0.0,0.0,0.0,0.0,0.0 +0,0.273800,0.0,0.0,0.0,0.0,0.0 +0,0.273900,0.0,0.0,0.0,0.0,0.0 +0,0.274000,0.0,0.0,0.0,0.0,0.0 +0,0.274100,0.0,0.0,0.0,0.0,0.0 +0,0.274200,0.0,0.0,0.0,0.0,0.0 +0,0.274300,0.0,0.0,0.0,0.0,0.0 +0,0.274400,0.0,0.0,0.0,0.0,0.0 +0,0.274500,0.0,0.0,0.0,0.0,0.0 +0,0.274600,0.0,0.0,0.0,0.0,0.0 +0,0.274700,0.0,0.0,0.0,0.0,0.0 +0,0.274800,0.0,0.0,0.0,0.0,0.0 +0,0.274900,0.0,0.0,0.0,0.0,0.0 +0,0.275000,0.0,0.0,0.0,0.0,0.0 +0,0.275100,0.0,0.0,0.0,0.0,0.0 +0,0.275200,0.0,0.0,0.0,0.0,0.0 +0,0.275300,0.0,0.0,0.0,0.0,0.0 +0,0.275400,0.0,0.0,0.0,0.0,0.0 +0,0.275500,0.0,0.0,0.0,0.0,0.0 +0,0.275600,0.0,0.0,0.0,0.0,0.0 +0,0.275700,0.0,0.0,0.0,0.0,0.0 +0,0.275800,0.0,0.0,0.0,0.0,0.0 +0,0.275900,0.0,0.0,0.0,0.0,0.0 +0,0.276000,0.0,0.0,0.0,0.0,0.0 +0,0.276100,0.0,0.0,0.0,0.0,0.0 +0,0.276200,0.0,0.0,0.0,0.0,0.0 +0,0.276300,0.0,0.0,0.0,0.0,0.0 +0,0.276400,0.0,0.0,0.0,0.0,0.0 +0,0.276500,0.0,0.0,0.0,0.0,0.0 +0,0.276600,0.0,0.0,0.0,0.0,0.0 +0,0.276700,0.0,0.0,0.0,0.0,0.0 +0,0.276800,0.0,0.0,0.0,0.0,0.0 +0,0.276900,0.0,0.0,0.0,0.0,0.0 +0,0.277000,0.0,0.0,0.0,0.0,0.0 +0,0.277100,0.0,0.0,0.0,0.0,0.0 +0,0.277200,0.0,0.0,0.0,0.0,0.0 +0,0.277300,0.0,0.0,0.0,0.0,0.0 +0,0.277400,0.0,0.0,0.0,0.0,0.0 +0,0.277500,0.0,0.0,0.0,0.0,0.0 +0,0.277600,0.0,0.0,0.0,0.0,0.0 +0,0.277700,0.0,0.0,0.0,0.0,0.0 +0,0.277800,0.0,0.0,0.0,0.0,0.0 +0,0.277900,0.0,0.0,0.0,0.0,0.0 +0,0.278000,0.0,0.0,0.0,0.0,0.0 +0,0.278100,0.0,0.0,0.0,0.0,0.0 +0,0.278200,0.0,0.0,0.0,0.0,0.0 +0,0.278300,0.0,0.0,0.0,0.0,0.0 +0,0.278400,0.0,0.0,0.0,0.0,0.0 +0,0.278500,0.0,0.0,0.0,0.0,0.0 +0,0.278600,0.0,0.0,0.0,0.0,0.0 +0,0.278700,0.0,0.0,0.0,0.0,0.0 +0,0.278800,0.0,0.0,0.0,0.0,0.0 +0,0.278900,0.0,0.0,0.0,0.0,0.0 +0,0.279000,0.0,0.0,0.0,0.0,0.0 +0,0.279100,0.0,0.0,0.0,0.0,0.0 +0,0.279200,0.0,0.0,0.0,0.0,0.0 +0,0.279300,0.0,0.0,0.0,0.0,0.0 +0,0.279400,0.0,0.0,0.0,0.0,0.0 +0,0.279500,0.0,0.0,0.0,0.0,0.0 +0,0.279600,0.0,0.0,0.0,0.0,0.0 +0,0.279700,0.0,0.0,0.0,0.0,0.0 +0,0.279800,0.0,0.0,0.0,0.0,0.0 +0,0.279900,0.0,0.0,0.0,0.0,0.0 +0,0.280000,0.0,0.0,0.0,0.0,0.0 +0,0.280100,0.0,0.0,0.0,0.0,0.0 +1,9.161374,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.280200,0.0,0.0,0.0,0.0,0.0 +0,0.280300,0.0,0.0,0.0,0.0,0.0 +0,0.280400,0.0,0.0,0.0,0.0,0.0 +0,0.280500,0.0,0.0,0.0,0.0,0.0 +0,0.280600,0.0,0.0,0.0,0.0,0.0 +0,0.280700,0.0,0.0,0.0,0.0,0.0 +0,0.280800,0.0,0.0,0.0,0.0,0.0 +0,0.280900,0.0,0.0,0.0,0.0,0.0 +0,0.281000,0.0,0.0,0.0,0.0,0.0 +0,0.281100,0.0,0.0,0.0,0.0,0.0 +0,0.281200,0.0,0.0,0.0,0.0,0.0 +0,0.281300,0.0,0.0,0.0,0.0,0.0 +0,0.281400,0.0,0.0,0.0,0.0,0.0 +0,0.281500,0.0,0.0,0.0,0.0,0.0 +0,0.281600,0.0,0.0,0.0,0.0,0.0 +0,0.281700,0.0,0.0,0.0,0.0,0.0 +0,0.281800,0.0,0.0,0.0,0.0,0.0 +0,0.281900,0.0,0.0,0.0,0.0,0.0 +0,0.282000,0.0,0.0,0.0,0.0,0.0 +0,0.282100,0.0,0.0,0.0,0.0,0.0 +0,0.282200,0.0,0.0,0.0,0.0,0.0 +0,0.282300,0.0,0.0,0.0,0.0,0.0 +0,0.282400,0.0,0.0,0.0,0.0,0.0 +0,0.282500,0.0,0.0,0.0,0.0,0.0 +0,0.282600,0.0,0.0,0.0,0.0,0.0 +0,0.282700,0.0,0.0,0.0,0.0,0.0 +0,0.282800,0.0,0.0,0.0,0.0,0.0 +0,0.282900,0.0,0.0,0.0,0.0,0.0 +0,0.283000,0.0,0.0,0.0,0.0,0.0 +0,0.283100,0.0,0.0,0.0,0.0,0.0 +0,0.283200,0.0,0.0,0.0,0.0,0.0 +0,0.283300,0.0,0.0,0.0,0.0,0.0 +0,0.283400,0.0,0.0,0.0,0.0,0.0 +0,0.283500,0.0,0.0,0.0,0.0,0.0 +0,0.283600,0.0,0.0,0.0,0.0,0.0 +0,0.283700,0.0,0.0,0.0,0.0,0.0 +0,0.283800,0.0,0.0,0.0,0.0,0.0 +0,0.283900,0.0,0.0,0.0,0.0,0.0 +0,0.284000,0.0,0.0,0.0,0.0,0.0 +0,0.284100,0.0,0.0,0.0,0.0,0.0 +0,0.284200,0.0,0.0,0.0,0.0,0.0 +0,0.284300,0.0,0.0,0.0,0.0,0.0 +0,0.284400,0.0,0.0,0.0,0.0,0.0 +0,0.284500,0.0,0.0,0.0,0.0,0.0 +0,0.284600,0.0,0.0,0.0,0.0,0.0 +0,0.284700,0.0,0.0,0.0,0.0,0.0 +0,0.284800,0.0,0.0,0.0,0.0,0.0 +0,0.284900,0.0,0.0,0.0,0.0,0.0 +0,0.285000,0.0,0.0,0.0,0.0,0.0 +0,0.285100,0.0,0.0,0.0,0.0,0.0 +0,0.285200,0.0,0.0,0.0,0.0,0.0 +0,0.285300,0.0,0.0,0.0,0.0,0.0 +0,0.285400,0.0,0.0,0.0,0.0,0.0 +0,0.285500,0.0,0.0,0.0,0.0,0.0 +0,0.285600,0.0,0.0,0.0,0.0,0.0 +0,0.285700,0.0,0.0,0.0,0.0,0.0 +0,0.285800,0.0,0.0,0.0,0.0,0.0 +0,0.285900,0.0,0.0,0.0,0.0,0.0 +0,0.286000,0.0,0.0,0.0,0.0,0.0 +0,0.286100,0.0,0.0,0.0,0.0,0.0 +0,0.286200,0.0,0.0,0.0,0.0,0.0 +0,0.286300,0.0,0.0,0.0,0.0,0.0 +0,0.286400,0.0,0.0,0.0,0.0,0.0 +0,0.286500,0.0,0.0,0.0,0.0,0.0 +0,0.286600,0.0,0.0,0.0,0.0,0.0 +0,0.286700,0.0,0.0,0.0,0.0,0.0 +0,0.286800,0.0,0.0,0.0,0.0,0.0 +0,0.286900,0.0,0.0,0.0,0.0,0.0 +0,0.287000,0.0,0.0,0.0,0.0,0.0 +0,0.287100,0.0,0.0,0.0,0.0,0.0 +0,0.287200,0.0,0.0,0.0,0.0,0.0 +0,0.287300,0.0,0.0,0.0,0.0,0.0 +0,0.287400,0.0,0.0,0.0,0.0,0.0 +0,0.287500,0.0,0.0,0.0,0.0,0.0 +0,0.287600,0.0,0.0,0.0,0.0,0.0 +0,0.287700,0.0,0.0,0.0,0.0,0.0 +0,0.287800,0.0,0.0,0.0,0.0,0.0 +0,0.287900,0.0,0.0,0.0,0.0,0.0 +0,0.288000,0.0,0.0,0.0,0.0,0.0 +0,0.288100,0.0,0.0,0.0,0.0,0.0 +0,0.288200,0.0,0.0,0.0,0.0,0.0 +0,0.288300,0.0,0.0,0.0,0.0,0.0 +0,0.288400,0.0,0.0,0.0,0.0,0.0 +0,0.288500,0.0,0.0,0.0,0.0,0.0 +0,0.288600,0.0,0.0,0.0,0.0,0.0 +0,0.288700,0.0,0.0,0.0,0.0,0.0 +0,0.288800,0.0,0.0,0.0,0.0,0.0 +0,0.288900,0.0,0.0,0.0,0.0,0.0 +0,0.289000,0.0,0.0,0.0,0.0,0.0 +0,0.289100,0.0,0.0,0.0,0.0,0.0 +0,0.289200,0.0,0.0,0.0,0.0,0.0 +0,0.289300,0.0,0.0,0.0,0.0,0.0 +0,0.289400,0.0,0.0,0.0,0.0,0.0 +0,0.289500,0.0,0.0,0.0,0.0,0.0 +0,0.289600,0.0,0.0,0.0,0.0,0.0 +0,0.289700,0.0,0.0,0.0,0.0,0.0 +0,0.289800,0.0,0.0,0.0,0.0,0.0 +0,0.289900,0.0,0.0,0.0,0.0,0.0 +0,0.290000,0.0,0.0,0.0,0.0,0.0 +0,0.290100,0.0,0.0,0.0,0.0,0.0 +0,0.290200,0.0,0.0,0.0,0.0,0.0 +0,0.290300,0.0,0.0,0.0,0.0,0.0 +0,0.290400,0.0,0.0,0.0,0.0,0.0 +0,0.290500,0.0,0.0,0.0,0.0,0.0 +0,0.290600,0.0,0.0,0.0,0.0,0.0 +0,0.290700,0.0,0.0,0.0,0.0,0.0 +0,0.290800,0.0,0.0,0.0,0.0,0.0 +0,0.290900,0.0,0.0,0.0,0.0,0.0 +0,0.291000,0.0,0.0,0.0,0.0,0.0 +0,0.291100,0.0,0.0,0.0,0.0,0.0 +0,0.291200,0.0,0.0,0.0,0.0,0.0 +0,0.291300,0.0,0.0,0.0,0.0,0.0 +0,0.291400,0.0,0.0,0.0,0.0,0.0 +0,0.291500,0.0,0.0,0.0,0.0,0.0 +0,0.291600,0.0,0.0,0.0,0.0,0.0 +0,0.291700,0.0,0.0,0.0,0.0,0.0 +0,0.291800,0.0,0.0,0.0,0.0,0.0 +0,0.291900,0.0,0.0,0.0,0.0,0.0 +0,0.292000,0.0,0.0,0.0,0.0,0.0 +0,0.292100,0.0,0.0,0.0,0.0,0.0 +0,0.292200,0.0,0.0,0.0,0.0,0.0 +0,0.292300,0.0,0.0,0.0,0.0,0.0 +0,0.292400,0.0,0.0,0.0,0.0,0.0 +0,0.292500,0.0,0.0,0.0,0.0,0.0 +0,0.292600,0.0,0.0,0.0,0.0,0.0 +0,0.292700,0.0,0.0,0.0,0.0,0.0 +0,0.292800,0.0,0.0,0.0,0.0,0.0 +0,0.292900,0.0,0.0,0.0,0.0,0.0 +0,0.293000,0.0,0.0,0.0,0.0,0.0 +0,0.293100,0.0,0.0,0.0,0.0,0.0 +0,0.293200,0.0,0.0,0.0,0.0,0.0 +0,0.293300,0.0,0.0,0.0,0.0,0.0 +0,0.293400,0.0,0.0,0.0,0.0,0.0 +0,0.293500,0.0,0.0,0.0,0.0,0.0 +0,0.293600,0.0,0.0,0.0,0.0,0.0 +0,0.293700,0.0,0.0,0.0,0.0,0.0 +0,0.293800,0.0,0.0,0.0,0.0,0.0 +0,0.293900,0.0,0.0,0.0,0.0,0.0 +0,0.294000,0.0,0.0,0.0,0.0,0.0 +0,0.294100,0.0,0.0,0.0,0.0,0.0 +0,0.294200,0.0,0.0,0.0,0.0,0.0 +0,0.294300,0.0,0.0,0.0,0.0,0.0 +0,0.294400,0.0,0.0,0.0,0.0,0.0 +0,0.294500,0.0,0.0,0.0,0.0,0.0 +0,0.294600,0.0,0.0,0.0,0.0,0.0 +0,0.294700,0.0,0.0,0.0,0.0,0.0 +0,0.294800,0.0,0.0,0.0,0.0,0.0 +0,0.294900,0.0,0.0,0.0,0.0,0.0 +0,0.295000,0.0,0.0,0.0,0.0,0.0 +0,0.295100,0.0,0.0,0.0,0.0,0.0 +0,0.295200,0.0,0.0,0.0,0.0,0.0 +0,0.295300,0.0,0.0,0.0,0.0,0.0 +0,0.295400,0.0,0.0,0.0,0.0,0.0 +0,0.295500,0.0,0.0,0.0,0.0,0.0 +0,0.295600,0.0,0.0,0.0,0.0,0.0 +0,0.295700,0.0,0.0,0.0,0.0,0.0 +0,0.295800,0.0,0.0,0.0,0.0,0.0 +0,0.295900,0.0,0.0,0.0,0.0,0.0 +0,0.296000,0.0,0.0,0.0,0.0,0.0 +0,0.296100,0.0,0.0,0.0,0.0,0.0 +0,0.296200,0.0,0.0,0.0,0.0,0.0 +0,0.296300,0.0,0.0,0.0,0.0,0.0 +0,0.296400,0.0,0.0,0.0,0.0,0.0 +0,0.296500,0.0,0.0,0.0,0.0,0.0 +0,0.296600,0.0,0.0,0.0,0.0,0.0 +0,0.296700,0.0,0.0,0.0,0.0,0.0 +0,0.296800,0.0,0.0,0.0,0.0,0.0 +0,0.296900,0.0,0.0,0.0,0.0,0.0 +0,0.297000,0.0,0.0,0.0,0.0,0.0 +0,0.297100,0.0,0.0,0.0,0.0,0.0 +0,0.297200,0.0,0.0,0.0,0.0,0.0 +0,0.297300,0.0,0.0,0.0,0.0,0.0 +0,0.297400,0.0,0.0,0.0,0.0,0.0 +0,0.297500,0.0,0.0,0.0,0.0,0.0 +0,0.297600,0.0,0.0,0.0,0.0,0.0 +0,0.297700,0.0,0.0,0.0,0.0,0.0 +0,0.297800,0.0,0.0,0.0,0.0,0.0 +0,0.297900,0.0,0.0,0.0,0.0,0.0 +0,0.298000,0.0,0.0,0.0,0.0,0.0 +0,0.298100,0.0,0.0,0.0,0.0,0.0 +0,0.298200,0.0,0.0,0.0,0.0,0.0 +0,0.298300,0.0,0.0,0.0,0.0,0.0 +0,0.298400,0.0,0.0,0.0,0.0,0.0 +0,0.298500,0.0,0.0,0.0,0.0,0.0 +0,0.298600,0.0,0.0,0.0,0.0,0.0 +0,0.298700,0.0,0.0,0.0,0.0,0.0 +0,0.298800,0.0,0.0,0.0,0.0,0.0 +0,0.298900,0.0,0.0,0.0,0.0,0.0 +0,0.299000,0.0,0.0,0.0,0.0,0.0 +0,0.299100,0.0,0.0,0.0,0.0,0.0 +0,0.299200,0.0,0.0,0.0,0.0,0.0 +0,0.299300,0.0,0.0,0.0,0.0,0.0 +0,0.299400,0.0,0.0,0.0,0.0,0.0 +0,0.299500,0.0,0.0,0.0,0.0,0.0 +0,0.299600,0.0,0.0,0.0,0.0,0.0 +0,0.299700,0.0,0.0,0.0,0.0,0.0 +0,0.299800,0.0,0.0,0.0,0.0,0.0 +0,0.299900,0.0,0.0,0.0,0.0,0.0 +0,0.300000,0.0,0.0,0.0,0.0,0.0 +0,0.300100,0.0,0.0,0.0,0.0,0.0 +1,11.266883,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.300200,0.0,0.0,0.0,0.0,0.0 +0,0.300300,0.0,0.0,0.0,0.0,0.0 +0,0.300400,0.0,0.0,0.0,0.0,0.0 +0,0.300500,0.0,0.0,0.0,0.0,0.0 +0,0.300600,0.0,0.0,0.0,0.0,0.0 +0,0.300700,0.0,0.0,0.0,0.0,0.0 +0,0.300800,0.0,0.0,0.0,0.0,0.0 +0,0.300900,0.0,0.0,0.0,0.0,0.0 +0,0.301000,0.0,0.0,0.0,0.0,0.0 +0,0.301100,0.0,0.0,0.0,0.0,0.0 +0,0.301200,0.0,0.0,0.0,0.0,0.0 +0,0.301300,0.0,0.0,0.0,0.0,0.0 +0,0.301400,0.0,0.0,0.0,0.0,0.0 +0,0.301500,0.0,0.0,0.0,0.0,0.0 +0,0.301600,0.0,0.0,0.0,0.0,0.0 +0,0.301700,0.0,0.0,0.0,0.0,0.0 +0,0.301800,0.0,0.0,0.0,0.0,0.0 +0,0.301900,0.0,0.0,0.0,0.0,0.0 +0,0.302000,0.0,0.0,0.0,0.0,0.0 +0,0.302100,0.0,0.0,0.0,0.0,0.0 +0,0.302200,0.0,0.0,0.0,0.0,0.0 +0,0.302300,0.0,0.0,0.0,0.0,0.0 +0,0.302400,0.0,0.0,0.0,0.0,0.0 +0,0.302500,0.0,0.0,0.0,0.0,0.0 +0,0.302600,0.0,0.0,0.0,0.0,0.0 +0,0.302700,0.0,0.0,0.0,0.0,0.0 +0,0.302800,0.0,0.0,0.0,0.0,0.0 +0,0.302900,0.0,0.0,0.0,0.0,0.0 +0,0.303000,0.0,0.0,0.0,0.0,0.0 +0,0.303100,0.0,0.0,0.0,0.0,0.0 +0,0.303200,0.0,0.0,0.0,0.0,0.0 +0,0.303300,0.0,0.0,0.0,0.0,0.0 +0,0.303400,0.0,0.0,0.0,0.0,0.0 +0,0.303500,0.0,0.0,0.0,0.0,0.0 +0,0.303600,0.0,0.0,0.0,0.0,0.0 +0,0.303700,0.0,0.0,0.0,0.0,0.0 +0,0.303800,0.0,0.0,0.0,0.0,0.0 +0,0.303900,0.0,0.0,0.0,0.0,0.0 +0,0.304000,0.0,0.0,0.0,0.0,0.0 +0,0.304100,0.0,0.0,0.0,0.0,0.0 +0,0.304200,0.0,0.0,0.0,0.0,0.0 +0,0.304300,0.0,0.0,0.0,0.0,0.0 +0,0.304400,0.0,0.0,0.0,0.0,0.0 +0,0.304500,0.0,0.0,0.0,0.0,0.0 +0,0.304600,0.0,0.0,0.0,0.0,0.0 +0,0.304700,0.0,0.0,0.0,0.0,0.0 +0,0.304800,0.0,0.0,0.0,0.0,0.0 +0,0.304900,0.0,0.0,0.0,0.0,0.0 +0,0.305000,0.0,0.0,0.0,0.0,0.0 +0,0.305100,0.0,0.0,0.0,0.0,0.0 +0,0.305200,0.0,0.0,0.0,0.0,0.0 +0,0.305300,0.0,0.0,0.0,0.0,0.0 +0,0.305400,0.0,0.0,0.0,0.0,0.0 +0,0.305500,0.0,0.0,0.0,0.0,0.0 +0,0.305600,0.0,0.0,0.0,0.0,0.0 +0,0.305700,0.0,0.0,0.0,0.0,0.0 +0,0.305800,0.0,0.0,0.0,0.0,0.0 +0,0.305900,0.0,0.0,0.0,0.0,0.0 +0,0.306000,0.0,0.0,0.0,0.0,0.0 +0,0.306100,0.0,0.0,0.0,0.0,0.0 +0,0.306200,0.0,0.0,0.0,0.0,0.0 +0,0.306300,0.0,0.0,0.0,0.0,0.0 +0,0.306400,0.0,0.0,0.0,0.0,0.0 +0,0.306500,0.0,0.0,0.0,0.0,0.0 +0,0.306600,0.0,0.0,0.0,0.0,0.0 +0,0.306700,0.0,0.0,0.0,0.0,0.0 +0,0.306800,0.0,0.0,0.0,0.0,0.0 +0,0.306900,0.0,0.0,0.0,0.0,0.0 +0,0.307000,0.0,0.0,0.0,0.0,0.0 +0,0.307100,0.0,0.0,0.0,0.0,0.0 +0,0.307200,0.0,0.0,0.0,0.0,0.0 +0,0.307300,0.0,0.0,0.0,0.0,0.0 +0,0.307400,0.0,0.0,0.0,0.0,0.0 +0,0.307500,0.0,0.0,0.0,0.0,0.0 +0,0.307600,0.0,0.0,0.0,0.0,0.0 +0,0.307700,0.0,0.0,0.0,0.0,0.0 +0,0.307800,0.0,0.0,0.0,0.0,0.0 +0,0.307900,0.0,0.0,0.0,0.0,0.0 +0,0.308000,0.0,0.0,0.0,0.0,0.0 +0,0.308100,0.0,0.0,0.0,0.0,0.0 +0,0.308200,0.0,0.0,0.0,0.0,0.0 +0,0.308300,0.0,0.0,0.0,0.0,0.0 +0,0.308400,0.0,0.0,0.0,0.0,0.0 +0,0.308500,0.0,0.0,0.0,0.0,0.0 +0,0.308600,0.0,0.0,0.0,0.0,0.0 +0,0.308700,0.0,0.0,0.0,0.0,0.0 +0,0.308800,0.0,0.0,0.0,0.0,0.0 +0,0.308900,0.0,0.0,0.0,0.0,0.0 +0,0.309000,0.0,0.0,0.0,0.0,0.0 +0,0.309100,0.0,0.0,0.0,0.0,0.0 +0,0.309200,0.0,0.0,0.0,0.0,0.0 +0,0.309300,0.0,0.0,0.0,0.0,0.0 +0,0.309400,0.0,0.0,0.0,0.0,0.0 +0,0.309500,0.0,0.0,0.0,0.0,0.0 +0,0.309600,0.0,0.0,0.0,0.0,0.0 +0,0.309700,0.0,0.0,0.0,0.0,0.0 +0,0.309800,0.0,0.0,0.0,0.0,0.0 +0,0.309900,0.0,0.0,0.0,0.0,0.0 +0,0.310000,0.0,0.0,0.0,0.0,0.0 +0,0.310100,0.0,0.0,0.0,0.0,0.0 +0,0.310200,0.0,0.0,0.0,0.0,0.0 +0,0.310300,0.0,0.0,0.0,0.0,0.0 +0,0.310400,0.0,0.0,0.0,0.0,0.0 +0,0.310500,0.0,0.0,0.0,0.0,0.0 +0,0.310600,0.0,0.0,0.0,0.0,0.0 +0,0.310700,0.0,0.0,0.0,0.0,0.0 +0,0.310800,0.0,0.0,0.0,0.0,0.0 +0,0.310900,0.0,0.0,0.0,0.0,0.0 +0,0.311000,0.0,0.0,0.0,0.0,0.0 +0,0.311100,0.0,0.0,0.0,0.0,0.0 +0,0.311200,0.0,0.0,0.0,0.0,0.0 +0,0.311300,0.0,0.0,0.0,0.0,0.0 +0,0.311400,0.0,0.0,0.0,0.0,0.0 +0,0.311500,0.0,0.0,0.0,0.0,0.0 +0,0.311600,0.0,0.0,0.0,0.0,0.0 +0,0.311700,0.0,0.0,0.0,0.0,0.0 +0,0.311800,0.0,0.0,0.0,0.0,0.0 +0,0.311900,0.0,0.0,0.0,0.0,0.0 +0,0.312000,0.0,0.0,0.0,0.0,0.0 +0,0.312100,0.0,0.0,0.0,0.0,0.0 +0,0.312200,0.0,0.0,0.0,0.0,0.0 +0,0.312300,0.0,0.0,0.0,0.0,0.0 +0,0.312400,0.0,0.0,0.0,0.0,0.0 +0,0.312500,0.0,0.0,0.0,0.0,0.0 +0,0.312600,0.0,0.0,0.0,0.0,0.0 +0,0.312700,0.0,0.0,0.0,0.0,0.0 +0,0.312800,0.0,0.0,0.0,0.0,0.0 +0,0.312900,0.0,0.0,0.0,0.0,0.0 +0,0.313000,0.0,0.0,0.0,0.0,0.0 +0,0.313100,0.0,0.0,0.0,0.0,0.0 +0,0.313200,0.0,0.0,0.0,0.0,0.0 +0,0.313300,0.0,0.0,0.0,0.0,0.0 +0,0.313400,0.0,0.0,0.0,0.0,0.0 +0,0.313500,0.0,0.0,0.0,0.0,0.0 +0,0.313600,0.0,0.0,0.0,0.0,0.0 +0,0.313700,0.0,0.0,0.0,0.0,0.0 +0,0.313800,0.0,0.0,0.0,0.0,0.0 +0,0.313900,0.0,0.0,0.0,0.0,0.0 +0,0.314000,0.0,0.0,0.0,0.0,0.0 +0,0.314100,0.0,0.0,0.0,0.0,0.0 +0,0.314200,0.0,0.0,0.0,0.0,0.0 +0,0.314300,0.0,0.0,0.0,0.0,0.0 +0,0.314400,0.0,0.0,0.0,0.0,0.0 +0,0.314500,0.0,0.0,0.0,0.0,0.0 +0,0.314600,0.0,0.0,0.0,0.0,0.0 +0,0.314700,0.0,0.0,0.0,0.0,0.0 +0,0.314800,0.0,0.0,0.0,0.0,0.0 +0,0.314900,0.0,0.0,0.0,0.0,0.0 +0,0.315000,0.0,0.0,0.0,0.0,0.0 +0,0.315100,0.0,0.0,0.0,0.0,0.0 +0,0.315200,0.0,0.0,0.0,0.0,0.0 +0,0.315300,0.0,0.0,0.0,0.0,0.0 +0,0.315400,0.0,0.0,0.0,0.0,0.0 +0,0.315500,0.0,0.0,0.0,0.0,0.0 +0,0.315600,0.0,0.0,0.0,0.0,0.0 +0,0.315700,0.0,0.0,0.0,0.0,0.0 +0,0.315800,0.0,0.0,0.0,0.0,0.0 +0,0.315900,0.0,0.0,0.0,0.0,0.0 +0,0.316000,0.0,0.0,0.0,0.0,0.0 +0,0.316100,0.0,0.0,0.0,0.0,0.0 +0,0.316200,0.0,0.0,0.0,0.0,0.0 +0,0.316300,0.0,0.0,0.0,0.0,0.0 +0,0.316400,0.0,0.0,0.0,0.0,0.0 +0,0.316500,0.0,0.0,0.0,0.0,0.0 +0,0.316600,0.0,0.0,0.0,0.0,0.0 +0,0.316700,0.0,0.0,0.0,0.0,0.0 +0,0.316800,0.0,0.0,0.0,0.0,0.0 +0,0.316900,0.0,0.0,0.0,0.0,0.0 +0,0.317000,0.0,0.0,0.0,0.0,0.0 +0,0.317100,0.0,0.0,0.0,0.0,0.0 +0,0.317200,0.0,0.0,0.0,0.0,0.0 +0,0.317300,0.0,0.0,0.0,0.0,0.0 +0,0.317400,0.0,0.0,0.0,0.0,0.0 +0,0.317500,0.0,0.0,0.0,0.0,0.0 +0,0.317600,0.0,0.0,0.0,0.0,0.0 +0,0.317700,0.0,0.0,0.0,0.0,0.0 +0,0.317800,0.0,0.0,0.0,0.0,0.0 +0,0.317900,0.0,0.0,0.0,0.0,0.0 +0,0.318000,0.0,0.0,0.0,0.0,0.0 +0,0.318100,0.0,0.0,0.0,0.0,0.0 +0,0.318200,0.0,0.0,0.0,0.0,0.0 +0,0.318300,0.0,0.0,0.0,0.0,0.0 +0,0.318400,0.0,0.0,0.0,0.0,0.0 +0,0.318500,0.0,0.0,0.0,0.0,0.0 +0,0.318600,0.0,0.0,0.0,0.0,0.0 +0,0.318700,0.0,0.0,0.0,0.0,0.0 +0,0.318800,0.0,0.0,0.0,0.0,0.0 +0,0.318900,0.0,0.0,0.0,0.0,0.0 +0,0.319000,0.0,0.0,0.0,0.0,0.0 +0,0.319100,0.0,0.0,0.0,0.0,0.0 +0,0.319200,0.0,0.0,0.0,0.0,0.0 +0,0.319300,0.0,0.0,0.0,0.0,0.0 +0,0.319400,0.0,0.0,0.0,0.0,0.0 +0,0.319500,0.0,0.0,0.0,0.0,0.0 +0,0.319600,0.0,0.0,0.0,0.0,0.0 +0,0.319700,0.0,0.0,0.0,0.0,0.0 +0,0.319800,0.0,0.0,0.0,0.0,0.0 +0,0.319900,0.0,0.0,0.0,0.0,0.0 +0,0.320000,0.0,0.0,0.0,0.0,0.0 +0,0.320100,0.0,0.0,0.0,0.0,0.0 +1,13.672542,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.320200,0.0,0.0,0.0,0.0,0.0 +0,0.320300,0.0,0.0,0.0,0.0,0.0 +0,0.320400,0.0,0.0,0.0,0.0,0.0 +0,0.320500,0.0,0.0,0.0,0.0,0.0 +0,0.320600,0.0,0.0,0.0,0.0,0.0 +0,0.320700,0.0,0.0,0.0,0.0,0.0 +0,0.320800,0.0,0.0,0.0,0.0,0.0 +0,0.320900,0.0,0.0,0.0,0.0,0.0 +0,0.321000,0.0,0.0,0.0,0.0,0.0 +0,0.321100,0.0,0.0,0.0,0.0,0.0 +0,0.321200,0.0,0.0,0.0,0.0,0.0 +0,0.321300,0.0,0.0,0.0,0.0,0.0 +0,0.321400,0.0,0.0,0.0,0.0,0.0 +0,0.321500,0.0,0.0,0.0,0.0,0.0 +0,0.321600,0.0,0.0,0.0,0.0,0.0 +0,0.321700,0.0,0.0,0.0,0.0,0.0 +0,0.321800,0.0,0.0,0.0,0.0,0.0 +0,0.321900,0.0,0.0,0.0,0.0,0.0 +0,0.322000,0.0,0.0,0.0,0.0,0.0 +0,0.322100,0.0,0.0,0.0,0.0,0.0 +0,0.322200,0.0,0.0,0.0,0.0,0.0 +0,0.322300,0.0,0.0,0.0,0.0,0.0 +0,0.322400,0.0,0.0,0.0,0.0,0.0 +0,0.322500,0.0,0.0,0.0,0.0,0.0 +0,0.322600,0.0,0.0,0.0,0.0,0.0 +0,0.322700,0.0,0.0,0.0,0.0,0.0 +0,0.322800,0.0,0.0,0.0,0.0,0.0 +0,0.322900,0.0,0.0,0.0,0.0,0.0 +0,0.323000,0.0,0.0,0.0,0.0,0.0 +0,0.323100,0.0,0.0,0.0,0.0,0.0 +0,0.323200,0.0,0.0,0.0,0.0,0.0 +0,0.323300,0.0,0.0,0.0,0.0,0.0 +0,0.323400,0.0,0.0,0.0,0.0,0.0 +0,0.323500,0.0,0.0,0.0,0.0,0.0 +0,0.323600,0.0,0.0,0.0,0.0,0.0 +0,0.323700,0.0,0.0,0.0,0.0,0.0 +0,0.323800,0.0,0.0,0.0,0.0,0.0 +0,0.323900,0.0,0.0,0.0,0.0,0.0 +0,0.324000,0.0,0.0,0.0,0.0,0.0 +0,0.324100,0.0,0.0,0.0,0.0,0.0 +0,0.324200,0.0,0.0,0.0,0.0,0.0 +0,0.324300,0.0,0.0,0.0,0.0,0.0 +0,0.324400,0.0,0.0,0.0,0.0,0.0 +0,0.324500,0.0,0.0,0.0,0.0,0.0 +0,0.324600,0.0,0.0,0.0,0.0,0.0 +0,0.324700,0.0,0.0,0.0,0.0,0.0 +0,0.324800,0.0,0.0,0.0,0.0,0.0 +0,0.324900,0.0,0.0,0.0,0.0,0.0 +0,0.325000,0.0,0.0,0.0,0.0,0.0 +0,0.325100,0.0,0.0,0.0,0.0,0.0 +0,0.325200,0.0,0.0,0.0,0.0,0.0 +0,0.325300,0.0,0.0,0.0,0.0,0.0 +0,0.325400,0.0,0.0,0.0,0.0,0.0 +0,0.325500,0.0,0.0,0.0,0.0,0.0 +0,0.325600,0.0,0.0,0.0,0.0,0.0 +0,0.325700,0.0,0.0,0.0,0.0,0.0 +0,0.325800,0.0,0.0,0.0,0.0,0.0 +0,0.325900,0.0,0.0,0.0,0.0,0.0 +0,0.326000,0.0,0.0,0.0,0.0,0.0 +0,0.326100,0.0,0.0,0.0,0.0,0.0 +0,0.326200,0.0,0.0,0.0,0.0,0.0 +0,0.326300,0.0,0.0,0.0,0.0,0.0 +0,0.326400,0.0,0.0,0.0,0.0,0.0 +0,0.326500,0.0,0.0,0.0,0.0,0.0 +0,0.326600,0.0,0.0,0.0,0.0,0.0 +0,0.326700,0.0,0.0,0.0,0.0,0.0 +0,0.326800,0.0,0.0,0.0,0.0,0.0 +0,0.326900,0.0,0.0,0.0,0.0,0.0 +0,0.327000,0.0,0.0,0.0,0.0,0.0 +0,0.327100,0.0,0.0,0.0,0.0,0.0 +0,0.327200,0.0,0.0,0.0,0.0,0.0 +0,0.327300,0.0,0.0,0.0,0.0,0.0 +0,0.327400,0.0,0.0,0.0,0.0,0.0 +0,0.327500,0.0,0.0,0.0,0.0,0.0 +0,0.327600,0.0,0.0,0.0,0.0,0.0 +0,0.327700,0.0,0.0,0.0,0.0,0.0 +0,0.327800,0.0,0.0,0.0,0.0,0.0 +0,0.327900,0.0,0.0,0.0,0.0,0.0 +0,0.328000,0.0,0.0,0.0,0.0,0.0 +0,0.328100,0.0,0.0,0.0,0.0,0.0 +0,0.328200,0.0,0.0,0.0,0.0,0.0 +0,0.328300,0.0,0.0,0.0,0.0,0.0 +0,0.328400,0.0,0.0,0.0,0.0,0.0 +0,0.328500,0.0,0.0,0.0,0.0,0.0 +0,0.328600,0.0,0.0,0.0,0.0,0.0 +0,0.328700,0.0,0.0,0.0,0.0,0.0 +0,0.328800,0.0,0.0,0.0,0.0,0.0 +0,0.328900,0.0,0.0,0.0,0.0,0.0 +0,0.329000,0.0,0.0,0.0,0.0,0.0 +0,0.329100,0.0,0.0,0.0,0.0,0.0 +0,0.329200,0.0,0.0,0.0,0.0,0.0 +0,0.329300,0.0,0.0,0.0,0.0,0.0 +0,0.329400,0.0,0.0,0.0,0.0,0.0 +0,0.329500,0.0,0.0,0.0,0.0,0.0 +0,0.329600,0.0,0.0,0.0,0.0,0.0 +0,0.329700,0.0,0.0,0.0,0.0,0.0 +0,0.329800,0.0,0.0,0.0,0.0,0.0 +0,0.329900,0.0,0.0,0.0,0.0,0.0 +0,0.330000,0.0,0.0,0.0,0.0,0.0 +0,0.330100,0.0,0.0,0.0,0.0,0.0 +0,0.330200,0.0,0.0,0.0,0.0,0.0 +0,0.330300,0.0,0.0,0.0,0.0,0.0 +0,0.330400,0.0,0.0,0.0,0.0,0.0 +0,0.330500,0.0,0.0,0.0,0.0,0.0 +0,0.330600,0.0,0.0,0.0,0.0,0.0 +0,0.330700,0.0,0.0,0.0,0.0,0.0 +0,0.330800,0.0,0.0,0.0,0.0,0.0 +0,0.330900,0.0,0.0,0.0,0.0,0.0 +0,0.331000,0.0,0.0,0.0,0.0,0.0 +0,0.331100,0.0,0.0,0.0,0.0,0.0 +0,0.331200,0.0,0.0,0.0,0.0,0.0 +0,0.331300,0.0,0.0,0.0,0.0,0.0 +0,0.331400,0.0,0.0,0.0,0.0,0.0 +0,0.331500,0.0,0.0,0.0,0.0,0.0 +0,0.331600,0.0,0.0,0.0,0.0,0.0 +0,0.331700,0.0,0.0,0.0,0.0,0.0 +0,0.331800,0.0,0.0,0.0,0.0,0.0 +0,0.331900,0.0,0.0,0.0,0.0,0.0 +0,0.332000,0.0,0.0,0.0,0.0,0.0 +0,0.332100,0.0,0.0,0.0,0.0,0.0 +0,0.332200,0.0,0.0,0.0,0.0,0.0 +0,0.332300,0.0,0.0,0.0,0.0,0.0 +0,0.332400,0.0,0.0,0.0,0.0,0.0 +0,0.332500,0.0,0.0,0.0,0.0,0.0 +0,0.332600,0.0,0.0,0.0,0.0,0.0 +0,0.332700,0.0,0.0,0.0,0.0,0.0 +0,0.332800,0.0,0.0,0.0,0.0,0.0 +0,0.332900,0.0,0.0,0.0,0.0,0.0 +0,0.333000,0.0,0.0,0.0,0.0,0.0 +0,0.333100,0.0,0.0,0.0,0.0,0.0 +0,0.333200,0.0,0.0,0.0,0.0,0.0 +0,0.333300,0.0,0.0,0.0,0.0,0.0 +0,0.333400,0.0,0.0,0.0,0.0,0.0 +0,0.333500,0.0,0.0,0.0,0.0,0.0 +0,0.333600,0.0,0.0,0.0,0.0,0.0 +0,0.333700,0.0,0.0,0.0,0.0,0.0 +0,0.333800,0.0,0.0,0.0,0.0,0.0 +0,0.333900,0.0,0.0,0.0,0.0,0.0 +0,0.334000,0.0,0.0,0.0,0.0,0.0 +0,0.334100,0.0,0.0,0.0,0.0,0.0 +0,0.334200,0.0,0.0,0.0,0.0,0.0 +0,0.334300,0.0,0.0,0.0,0.0,0.0 +0,0.334400,0.0,0.0,0.0,0.0,0.0 +0,0.334500,0.0,0.0,0.0,0.0,0.0 +0,0.334600,0.0,0.0,0.0,0.0,0.0 +0,0.334700,0.0,0.0,0.0,0.0,0.0 +0,0.334800,0.0,0.0,0.0,0.0,0.0 +0,0.334900,0.0,0.0,0.0,0.0,0.0 +0,0.335000,0.0,0.0,0.0,0.0,0.0 +0,0.335100,0.0,0.0,0.0,0.0,0.0 +0,0.335200,0.0,0.0,0.0,0.0,0.0 +0,0.335300,0.0,0.0,0.0,0.0,0.0 +0,0.335400,0.0,0.0,0.0,0.0,0.0 +0,0.335500,0.0,0.0,0.0,0.0,0.0 +0,0.335600,0.0,0.0,0.0,0.0,0.0 +0,0.335700,0.0,0.0,0.0,0.0,0.0 +0,0.335800,0.0,0.0,0.0,0.0,0.0 +0,0.335900,0.0,0.0,0.0,0.0,0.0 +0,0.336000,0.0,0.0,0.0,0.0,0.0 +0,0.336100,0.0,0.0,0.0,0.0,0.0 +0,0.336200,0.0,0.0,0.0,0.0,0.0 +0,0.336300,0.0,0.0,0.0,0.0,0.0 +0,0.336400,0.0,0.0,0.0,0.0,0.0 +0,0.336500,0.0,0.0,0.0,0.0,0.0 +0,0.336600,0.0,0.0,0.0,0.0,0.0 +0,0.336700,0.0,0.0,0.0,0.0,0.0 +0,0.336800,0.0,0.0,0.0,0.0,0.0 +0,0.336900,0.0,0.0,0.0,0.0,0.0 +0,0.337000,0.0,0.0,0.0,0.0,0.0 +0,0.337100,0.0,0.0,0.0,0.0,0.0 +0,0.337200,0.0,0.0,0.0,0.0,0.0 +0,0.337300,0.0,0.0,0.0,0.0,0.0 +0,0.337400,0.0,0.0,0.0,0.0,0.0 +0,0.337500,0.0,0.0,0.0,0.0,0.0 +0,0.337600,0.0,0.0,0.0,0.0,0.0 +0,0.337700,0.0,0.0,0.0,0.0,0.0 +0,0.337800,0.0,0.0,0.0,0.0,0.0 +0,0.337900,0.0,0.0,0.0,0.0,0.0 +0,0.338000,0.0,0.0,0.0,0.0,0.0 +0,0.338100,0.0,0.0,0.0,0.0,0.0 +0,0.338200,0.0,0.0,0.0,0.0,0.0 +0,0.338300,0.0,0.0,0.0,0.0,0.0 +0,0.338400,0.0,0.0,0.0,0.0,0.0 +0,0.338500,0.0,0.0,0.0,0.0,0.0 +0,0.338600,0.0,0.0,0.0,0.0,0.0 +0,0.338700,0.0,0.0,0.0,0.0,0.0 +0,0.338800,0.0,0.0,0.0,0.0,0.0 +0,0.338900,0.0,0.0,0.0,0.0,0.0 +0,0.339000,0.0,0.0,0.0,0.0,0.0 +0,0.339100,0.0,0.0,0.0,0.0,0.0 +0,0.339200,0.0,0.0,0.0,0.0,0.0 +0,0.339300,0.0,0.0,0.0,0.0,0.0 +0,0.339400,0.0,0.0,0.0,0.0,0.0 +0,0.339500,0.0,0.0,0.0,0.0,0.0 +0,0.339600,0.0,0.0,0.0,0.0,0.0 +0,0.339700,0.0,0.0,0.0,0.0,0.0 +0,0.339800,0.0,0.0,0.0,0.0,0.0 +0,0.339900,0.0,0.0,0.0,0.0,0.0 +0,0.340000,0.0,0.0,0.0,0.0,0.0 +0,0.340100,0.0,0.0,0.0,0.0,0.0 +1,16.398351,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.340200,0.0,0.0,0.0,0.0,0.0 +0,0.340300,0.0,0.0,0.0,0.0,0.0 +0,0.340400,0.0,0.0,0.0,0.0,0.0 +0,0.340500,0.0,0.0,0.0,0.0,0.0 +0,0.340600,0.0,0.0,0.0,0.0,0.0 +0,0.340700,0.0,0.0,0.0,0.0,0.0 +0,0.340800,0.0,0.0,0.0,0.0,0.0 +0,0.340900,0.0,0.0,0.0,0.0,0.0 +0,0.341000,0.0,0.0,0.0,0.0,0.0 +0,0.341100,0.0,0.0,0.0,0.0,0.0 +0,0.341200,0.0,0.0,0.0,0.0,0.0 +0,0.341300,0.0,0.0,0.0,0.0,0.0 +0,0.341400,0.0,0.0,0.0,0.0,0.0 +0,0.341500,0.0,0.0,0.0,0.0,0.0 +0,0.341600,0.0,0.0,0.0,0.0,0.0 +0,0.341700,0.0,0.0,0.0,0.0,0.0 +0,0.341800,0.0,0.0,0.0,0.0,0.0 +0,0.341900,0.0,0.0,0.0,0.0,0.0 +0,0.342000,0.0,0.0,0.0,0.0,0.0 +0,0.342100,0.0,0.0,0.0,0.0,0.0 +0,0.342200,0.0,0.0,0.0,0.0,0.0 +0,0.342300,0.0,0.0,0.0,0.0,0.0 +0,0.342400,0.0,0.0,0.0,0.0,0.0 +0,0.342500,0.0,0.0,0.0,0.0,0.0 +0,0.342600,0.0,0.0,0.0,0.0,0.0 +0,0.342700,0.0,0.0,0.0,0.0,0.0 +0,0.342800,0.0,0.0,0.0,0.0,0.0 +0,0.342900,0.0,0.0,0.0,0.0,0.0 +0,0.343000,0.0,0.0,0.0,0.0,0.0 +0,0.343100,0.0,0.0,0.0,0.0,0.0 +0,0.343200,0.0,0.0,0.0,0.0,0.0 +0,0.343300,0.0,0.0,0.0,0.0,0.0 +0,0.343400,0.0,0.0,0.0,0.0,0.0 +0,0.343500,0.0,0.0,0.0,0.0,0.0 +0,0.343600,0.0,0.0,0.0,0.0,0.0 +0,0.343700,0.0,0.0,0.0,0.0,0.0 +0,0.343800,0.0,0.0,0.0,0.0,0.0 +0,0.343900,0.0,0.0,0.0,0.0,0.0 +0,0.344000,0.0,0.0,0.0,0.0,0.0 +0,0.344100,0.0,0.0,0.0,0.0,0.0 +0,0.344200,0.0,0.0,0.0,0.0,0.0 +0,0.344300,0.0,0.0,0.0,0.0,0.0 +0,0.344400,0.0,0.0,0.0,0.0,0.0 +0,0.344500,0.0,0.0,0.0,0.0,0.0 +0,0.344600,0.0,0.0,0.0,0.0,0.0 +0,0.344700,0.0,0.0,0.0,0.0,0.0 +0,0.344800,0.0,0.0,0.0,0.0,0.0 +0,0.344900,0.0,0.0,0.0,0.0,0.0 +0,0.345000,0.0,0.0,0.0,0.0,0.0 +0,0.345100,0.0,0.0,0.0,0.0,0.0 +0,0.345200,0.0,0.0,0.0,0.0,0.0 +0,0.345300,0.0,0.0,0.0,0.0,0.0 +0,0.345400,0.0,0.0,0.0,0.0,0.0 +0,0.345500,0.0,0.0,0.0,0.0,0.0 +0,0.345600,0.0,0.0,0.0,0.0,0.0 +0,0.345700,0.0,0.0,0.0,0.0,0.0 +0,0.345800,0.0,0.0,0.0,0.0,0.0 +0,0.345900,0.0,0.0,0.0,0.0,0.0 +0,0.346000,0.0,0.0,0.0,0.0,0.0 +0,0.346100,0.0,0.0,0.0,0.0,0.0 +0,0.346200,0.0,0.0,0.0,0.0,0.0 +0,0.346300,0.0,0.0,0.0,0.0,0.0 +0,0.346400,0.0,0.0,0.0,0.0,0.0 +0,0.346500,0.0,0.0,0.0,0.0,0.0 +0,0.346600,0.0,0.0,0.0,0.0,0.0 +0,0.346700,0.0,0.0,0.0,0.0,0.0 +0,0.346800,0.0,0.0,0.0,0.0,0.0 +0,0.346900,0.0,0.0,0.0,0.0,0.0 +0,0.347000,0.0,0.0,0.0,0.0,0.0 +0,0.347100,0.0,0.0,0.0,0.0,0.0 +0,0.347200,0.0,0.0,0.0,0.0,0.0 +0,0.347300,0.0,0.0,0.0,0.0,0.0 +0,0.347400,0.0,0.0,0.0,0.0,0.0 +0,0.347500,0.0,0.0,0.0,0.0,0.0 +0,0.347600,0.0,0.0,0.0,0.0,0.0 +0,0.347700,0.0,0.0,0.0,0.0,0.0 +0,0.347800,0.0,0.0,0.0,0.0,0.0 +0,0.347900,0.0,0.0,0.0,0.0,0.0 +0,0.348000,0.0,0.0,0.0,0.0,0.0 +0,0.348100,0.0,0.0,0.0,0.0,0.0 +0,0.348200,0.0,0.0,0.0,0.0,0.0 +0,0.348300,0.0,0.0,0.0,0.0,0.0 +0,0.348400,0.0,0.0,0.0,0.0,0.0 +0,0.348500,0.0,0.0,0.0,0.0,0.0 +0,0.348600,0.0,0.0,0.0,0.0,0.0 +0,0.348700,0.0,0.0,0.0,0.0,0.0 +0,0.348800,0.0,0.0,0.0,0.0,0.0 +0,0.348900,0.0,0.0,0.0,0.0,0.0 +0,0.349000,0.0,0.0,0.0,0.0,0.0 +0,0.349100,0.0,0.0,0.0,0.0,0.0 +0,0.349200,0.0,0.0,0.0,0.0,0.0 +0,0.349300,0.0,0.0,0.0,0.0,0.0 +0,0.349400,0.0,0.0,0.0,0.0,0.0 +0,0.349500,0.0,0.0,0.0,0.0,0.0 +0,0.349600,0.0,0.0,0.0,0.0,0.0 +0,0.349700,0.0,0.0,0.0,0.0,0.0 +0,0.349800,0.0,0.0,0.0,0.0,0.0 +0,0.349900,0.0,0.0,0.0,0.0,0.0 +0,0.350000,0.0,0.0,0.0,0.0,0.0 +0,0.350100,0.0,0.0,0.0,0.0,0.0 +0,0.350200,0.0,0.0,0.0,0.0,0.0 +0,0.350300,0.0,0.0,0.0,0.0,0.0 +0,0.350400,0.0,0.0,0.0,0.0,0.0 +0,0.350500,0.0,0.0,0.0,0.0,0.0 +0,0.350600,0.0,0.0,0.0,0.0,0.0 +0,0.350700,0.0,0.0,0.0,0.0,0.0 +0,0.350800,0.0,0.0,0.0,0.0,0.0 +0,0.350900,0.0,0.0,0.0,0.0,0.0 +0,0.351000,0.0,0.0,0.0,0.0,0.0 +0,0.351100,0.0,0.0,0.0,0.0,0.0 +0,0.351200,0.0,0.0,0.0,0.0,0.0 +0,0.351300,0.0,0.0,0.0,0.0,0.0 +0,0.351400,0.0,0.0,0.0,0.0,0.0 +0,0.351500,0.0,0.0,0.0,0.0,0.0 +0,0.351600,0.0,0.0,0.0,0.0,0.0 +0,0.351700,0.0,0.0,0.0,0.0,0.0 +0,0.351800,0.0,0.0,0.0,0.0,0.0 +0,0.351900,0.0,0.0,0.0,0.0,0.0 +0,0.352000,0.0,0.0,0.0,0.0,0.0 +0,0.352100,0.0,0.0,0.0,0.0,0.0 +0,0.352200,0.0,0.0,0.0,0.0,0.0 +0,0.352300,0.0,0.0,0.0,0.0,0.0 +0,0.352400,0.0,0.0,0.0,0.0,0.0 +0,0.352500,0.0,0.0,0.0,0.0,0.0 +0,0.352600,0.0,0.0,0.0,0.0,0.0 +0,0.352700,0.0,0.0,0.0,0.0,0.0 +0,0.352800,0.0,0.0,0.0,0.0,0.0 +0,0.352900,0.0,0.0,0.0,0.0,0.0 +0,0.353000,0.0,0.0,0.0,0.0,0.0 +0,0.353100,0.0,0.0,0.0,0.0,0.0 +0,0.353200,0.0,0.0,0.0,0.0,0.0 +0,0.353300,0.0,0.0,0.0,0.0,0.0 +0,0.353400,0.0,0.0,0.0,0.0,0.0 +0,0.353500,0.0,0.0,0.0,0.0,0.0 +0,0.353600,0.0,0.0,0.0,0.0,0.0 +0,0.353700,0.0,0.0,0.0,0.0,0.0 +0,0.353800,0.0,0.0,0.0,0.0,0.0 +0,0.353900,0.0,0.0,0.0,0.0,0.0 +0,0.354000,0.0,0.0,0.0,0.0,0.0 +0,0.354100,0.0,0.0,0.0,0.0,0.0 +0,0.354200,0.0,0.0,0.0,0.0,0.0 +0,0.354300,0.0,0.0,0.0,0.0,0.0 +0,0.354400,0.0,0.0,0.0,0.0,0.0 +0,0.354500,0.0,0.0,0.0,0.0,0.0 +0,0.354600,0.0,0.0,0.0,0.0,0.0 +0,0.354700,0.0,0.0,0.0,0.0,0.0 +0,0.354800,0.0,0.0,0.0,0.0,0.0 +0,0.354900,0.0,0.0,0.0,0.0,0.0 +0,0.355000,0.0,0.0,0.0,0.0,0.0 +0,0.355100,0.0,0.0,0.0,0.0,0.0 +0,0.355200,0.0,0.0,0.0,0.0,0.0 +0,0.355300,0.0,0.0,0.0,0.0,0.0 +0,0.355400,0.0,0.0,0.0,0.0,0.0 +0,0.355500,0.0,0.0,0.0,0.0,0.0 +0,0.355600,0.0,0.0,0.0,0.0,0.0 +0,0.355700,0.0,0.0,0.0,0.0,0.0 +0,0.355800,0.0,0.0,0.0,0.0,0.0 +0,0.355900,0.0,0.0,0.0,0.0,0.0 +0,0.356000,0.0,0.0,0.0,0.0,0.0 +0,0.356100,0.0,0.0,0.0,0.0,0.0 +0,0.356200,0.0,0.0,0.0,0.0,0.0 +0,0.356300,0.0,0.0,0.0,0.0,0.0 +0,0.356400,0.0,0.0,0.0,0.0,0.0 +0,0.356500,0.0,0.0,0.0,0.0,0.0 +0,0.356600,0.0,0.0,0.0,0.0,0.0 +0,0.356700,0.0,0.0,0.0,0.0,0.0 +0,0.356800,0.0,0.0,0.0,0.0,0.0 +0,0.356900,0.0,0.0,0.0,0.0,0.0 +0,0.357000,0.0,0.0,0.0,0.0,0.0 +0,0.357100,0.0,0.0,0.0,0.0,0.0 +0,0.357200,0.0,0.0,0.0,0.0,0.0 +0,0.357300,0.0,0.0,0.0,0.0,0.0 +0,0.357400,0.0,0.0,0.0,0.0,0.0 +0,0.357500,0.0,0.0,0.0,0.0,0.0 +0,0.357600,0.0,0.0,0.0,0.0,0.0 +0,0.357700,0.0,0.0,0.0,0.0,0.0 +0,0.357800,0.0,0.0,0.0,0.0,0.0 +0,0.357900,0.0,0.0,0.0,0.0,0.0 +0,0.358000,0.0,0.0,0.0,0.0,0.0 +0,0.358100,0.0,0.0,0.0,0.0,0.0 +0,0.358200,0.0,0.0,0.0,0.0,0.0 +0,0.358300,0.0,0.0,0.0,0.0,0.0 +0,0.358400,0.0,0.0,0.0,0.0,0.0 +0,0.358500,0.0,0.0,0.0,0.0,0.0 +0,0.358600,0.0,0.0,0.0,0.0,0.0 +0,0.358700,0.0,0.0,0.0,0.0,0.0 +0,0.358800,0.0,0.0,0.0,0.0,0.0 +0,0.358900,0.0,0.0,0.0,0.0,0.0 +0,0.359000,0.0,0.0,0.0,0.0,0.0 +0,0.359100,0.0,0.0,0.0,0.0,0.0 +0,0.359200,0.0,0.0,0.0,0.0,0.0 +0,0.359300,0.0,0.0,0.0,0.0,0.0 +0,0.359400,0.0,0.0,0.0,0.0,0.0 +0,0.359500,0.0,0.0,0.0,0.0,0.0 +0,0.359600,0.0,0.0,0.0,0.0,0.0 +0,0.359700,0.0,0.0,0.0,0.0,0.0 +0,0.359800,0.0,0.0,0.0,0.0,0.0 +0,0.359900,0.0,0.0,0.0,0.0,0.0 +0,0.360000,0.0,0.0,0.0,0.0,0.0 +0,0.360100,0.0,0.0,0.0,0.0,0.0 +1,19.464310,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.360200,0.0,0.0,0.0,0.0,0.0 +0,0.360300,0.0,0.0,0.0,0.0,0.0 +0,0.360400,0.0,0.0,0.0,0.0,0.0 +0,0.360500,0.0,0.0,0.0,0.0,0.0 +0,0.360600,0.0,0.0,0.0,0.0,0.0 +0,0.360700,0.0,0.0,0.0,0.0,0.0 +0,0.360800,0.0,0.0,0.0,0.0,0.0 +0,0.360900,0.0,0.0,0.0,0.0,0.0 +0,0.361000,0.0,0.0,0.0,0.0,0.0 +0,0.361100,0.0,0.0,0.0,0.0,0.0 +0,0.361200,0.0,0.0,0.0,0.0,0.0 +0,0.361300,0.0,0.0,0.0,0.0,0.0 +0,0.361400,0.0,0.0,0.0,0.0,0.0 +0,0.361500,0.0,0.0,0.0,0.0,0.0 +0,0.361600,0.0,0.0,0.0,0.0,0.0 +0,0.361700,0.0,0.0,0.0,0.0,0.0 +0,0.361800,0.0,0.0,0.0,0.0,0.0 +0,0.361900,0.0,0.0,0.0,0.0,0.0 +0,0.362000,0.0,0.0,0.0,0.0,0.0 +0,0.362100,0.0,0.0,0.0,0.0,0.0 +0,0.362200,0.0,0.0,0.0,0.0,0.0 +0,0.362300,0.0,0.0,0.0,0.0,0.0 +0,0.362400,0.0,0.0,0.0,0.0,0.0 +0,0.362500,0.0,0.0,0.0,0.0,0.0 +0,0.362600,0.0,0.0,0.0,0.0,0.0 +0,0.362700,0.0,0.0,0.0,0.0,0.0 +0,0.362800,0.0,0.0,0.0,0.0,0.0 +0,0.362900,0.0,0.0,0.0,0.0,0.0 +0,0.363000,0.0,0.0,0.0,0.0,0.0 +0,0.363100,0.0,0.0,0.0,0.0,0.0 +0,0.363200,0.0,0.0,0.0,0.0,0.0 +0,0.363300,0.0,0.0,0.0,0.0,0.0 +0,0.363400,0.0,0.0,0.0,0.0,0.0 +0,0.363500,0.0,0.0,0.0,0.0,0.0 +0,0.363600,0.0,0.0,0.0,0.0,0.0 +0,0.363700,0.0,0.0,0.0,0.0,0.0 +0,0.363800,0.0,0.0,0.0,0.0,0.0 +0,0.363900,0.0,0.0,0.0,0.0,0.0 +0,0.364000,0.0,0.0,0.0,0.0,0.0 +0,0.364100,0.0,0.0,0.0,0.0,0.0 +0,0.364200,0.0,0.0,0.0,0.0,0.0 +0,0.364300,0.0,0.0,0.0,0.0,0.0 +0,0.364400,0.0,0.0,0.0,0.0,0.0 +0,0.364500,0.0,0.0,0.0,0.0,0.0 +0,0.364600,0.0,0.0,0.0,0.0,0.0 +0,0.364700,0.0,0.0,0.0,0.0,0.0 +0,0.364800,0.0,0.0,0.0,0.0,0.0 +0,0.364900,0.0,0.0,0.0,0.0,0.0 +0,0.365000,0.0,0.0,0.0,0.0,0.0 +0,0.365100,0.0,0.0,0.0,0.0,0.0 +0,0.365200,0.0,0.0,0.0,0.0,0.0 +0,0.365300,0.0,0.0,0.0,0.0,0.0 +0,0.365400,0.0,0.0,0.0,0.0,0.0 +0,0.365500,0.0,0.0,0.0,0.0,0.0 +0,0.365600,0.0,0.0,0.0,0.0,0.0 +0,0.365700,0.0,0.0,0.0,0.0,0.0 +0,0.365800,0.0,0.0,0.0,0.0,0.0 +0,0.365900,0.0,0.0,0.0,0.0,0.0 +0,0.366000,0.0,0.0,0.0,0.0,0.0 +0,0.366100,0.0,0.0,0.0,0.0,0.0 +0,0.366200,0.0,0.0,0.0,0.0,0.0 +0,0.366300,0.0,0.0,0.0,0.0,0.0 +0,0.366400,0.0,0.0,0.0,0.0,0.0 +0,0.366500,0.0,0.0,0.0,0.0,0.0 +0,0.366600,0.0,0.0,0.0,0.0,0.0 +0,0.366700,0.0,0.0,0.0,0.0,0.0 +0,0.366800,0.0,0.0,0.0,0.0,0.0 +0,0.366900,0.0,0.0,0.0,0.0,0.0 +0,0.367000,0.0,0.0,0.0,0.0,0.0 +0,0.367100,0.0,0.0,0.0,0.0,0.0 +0,0.367200,0.0,0.0,0.0,0.0,0.0 +0,0.367300,0.0,0.0,0.0,0.0,0.0 +0,0.367400,0.0,0.0,0.0,0.0,0.0 +0,0.367500,0.0,0.0,0.0,0.0,0.0 +0,0.367600,0.0,0.0,0.0,0.0,0.0 +0,0.367700,0.0,0.0,0.0,0.0,0.0 +0,0.367800,0.0,0.0,0.0,0.0,0.0 +0,0.367900,0.0,0.0,0.0,0.0,0.0 +0,0.368000,0.0,0.0,0.0,0.0,0.0 +0,0.368100,0.0,0.0,0.0,0.0,0.0 +0,0.368200,0.0,0.0,0.0,0.0,0.0 +0,0.368300,0.0,0.0,0.0,0.0,0.0 +0,0.368400,0.0,0.0,0.0,0.0,0.0 +0,0.368500,0.0,0.0,0.0,0.0,0.0 +0,0.368600,0.0,0.0,0.0,0.0,0.0 +0,0.368700,0.0,0.0,0.0,0.0,0.0 +0,0.368800,0.0,0.0,0.0,0.0,0.0 +0,0.368900,0.0,0.0,0.0,0.0,0.0 +0,0.369000,0.0,0.0,0.0,0.0,0.0 +0,0.369100,0.0,0.0,0.0,0.0,0.0 +0,0.369200,0.0,0.0,0.0,0.0,0.0 +0,0.369300,0.0,0.0,0.0,0.0,0.0 +0,0.369400,0.0,0.0,0.0,0.0,0.0 +0,0.369500,0.0,0.0,0.0,0.0,0.0 +0,0.369600,0.0,0.0,0.0,0.0,0.0 +0,0.369700,0.0,0.0,0.0,0.0,0.0 +0,0.369800,0.0,0.0,0.0,0.0,0.0 +0,0.369900,0.0,0.0,0.0,0.0,0.0 +0,0.370000,0.0,0.0,0.0,0.0,0.0 +0,0.370100,0.0,0.0,0.0,0.0,0.0 +0,0.370200,0.0,0.0,0.0,0.0,0.0 +0,0.370300,0.0,0.0,0.0,0.0,0.0 +0,0.370400,0.0,0.0,0.0,0.0,0.0 +0,0.370500,0.0,0.0,0.0,0.0,0.0 +0,0.370600,0.0,0.0,0.0,0.0,0.0 +0,0.370700,0.0,0.0,0.0,0.0,0.0 +0,0.370800,0.0,0.0,0.0,0.0,0.0 +0,0.370900,0.0,0.0,0.0,0.0,0.0 +0,0.371000,0.0,0.0,0.0,0.0,0.0 +0,0.371100,0.0,0.0,0.0,0.0,0.0 +0,0.371200,0.0,0.0,0.0,0.0,0.0 +0,0.371300,0.0,0.0,0.0,0.0,0.0 +0,0.371400,0.0,0.0,0.0,0.0,0.0 +0,0.371500,0.0,0.0,0.0,0.0,0.0 +0,0.371600,0.0,0.0,0.0,0.0,0.0 +0,0.371700,0.0,0.0,0.0,0.0,0.0 +0,0.371800,0.0,0.0,0.0,0.0,0.0 +0,0.371900,0.0,0.0,0.0,0.0,0.0 +0,0.372000,0.0,0.0,0.0,0.0,0.0 +0,0.372100,0.0,0.0,0.0,0.0,0.0 +0,0.372200,0.0,0.0,0.0,0.0,0.0 +0,0.372300,0.0,0.0,0.0,0.0,0.0 +0,0.372400,0.0,0.0,0.0,0.0,0.0 +0,0.372500,0.0,0.0,0.0,0.0,0.0 +0,0.372600,0.0,0.0,0.0,0.0,0.0 +0,0.372700,0.0,0.0,0.0,0.0,0.0 +0,0.372800,0.0,0.0,0.0,0.0,0.0 +0,0.372900,0.0,0.0,0.0,0.0,0.0 +0,0.373000,0.0,0.0,0.0,0.0,0.0 +0,0.373100,0.0,0.0,0.0,0.0,0.0 +0,0.373200,0.0,0.0,0.0,0.0,0.0 +0,0.373300,0.0,0.0,0.0,0.0,0.0 +0,0.373400,0.0,0.0,0.0,0.0,0.0 +0,0.373500,0.0,0.0,0.0,0.0,0.0 +0,0.373600,0.0,0.0,0.0,0.0,0.0 +0,0.373700,0.0,0.0,0.0,0.0,0.0 +0,0.373800,0.0,0.0,0.0,0.0,0.0 +0,0.373900,0.0,0.0,0.0,0.0,0.0 +0,0.374000,0.0,0.0,0.0,0.0,0.0 +0,0.374100,0.0,0.0,0.0,0.0,0.0 +0,0.374200,0.0,0.0,0.0,0.0,0.0 +0,0.374300,0.0,0.0,0.0,0.0,0.0 +0,0.374400,0.0,0.0,0.0,0.0,0.0 +0,0.374500,0.0,0.0,0.0,0.0,0.0 +0,0.374600,0.0,0.0,0.0,0.0,0.0 +0,0.374700,0.0,0.0,0.0,0.0,0.0 +0,0.374800,0.0,0.0,0.0,0.0,0.0 +0,0.374900,0.0,0.0,0.0,0.0,0.0 +0,0.375000,0.0,0.0,0.0,0.0,0.0 +0,0.375100,0.0,0.0,0.0,0.0,0.0 +0,0.375200,0.0,0.0,0.0,0.0,0.0 +0,0.375300,0.0,0.0,0.0,0.0,0.0 +0,0.375400,0.0,0.0,0.0,0.0,0.0 +0,0.375500,0.0,0.0,0.0,0.0,0.0 +0,0.375600,0.0,0.0,0.0,0.0,0.0 +0,0.375700,0.0,0.0,0.0,0.0,0.0 +0,0.375800,0.0,0.0,0.0,0.0,0.0 +0,0.375900,0.0,0.0,0.0,0.0,0.0 +0,0.376000,0.0,0.0,0.0,0.0,0.0 +0,0.376100,0.0,0.0,0.0,0.0,0.0 +0,0.376200,0.0,0.0,0.0,0.0,0.0 +0,0.376300,0.0,0.0,0.0,0.0,0.0 +0,0.376400,0.0,0.0,0.0,0.0,0.0 +0,0.376500,0.0,0.0,0.0,0.0,0.0 +0,0.376600,0.0,0.0,0.0,0.0,0.0 +0,0.376700,0.0,0.0,0.0,0.0,0.0 +0,0.376800,0.0,0.0,0.0,0.0,0.0 +0,0.376900,0.0,0.0,0.0,0.0,0.0 +0,0.377000,0.0,0.0,0.0,0.0,0.0 +0,0.377100,0.0,0.0,0.0,0.0,0.0 +0,0.377200,0.0,0.0,0.0,0.0,0.0 +0,0.377300,0.0,0.0,0.0,0.0,0.0 +0,0.377400,0.0,0.0,0.0,0.0,0.0 +0,0.377500,0.0,0.0,0.0,0.0,0.0 +0,0.377600,0.0,0.0,0.0,0.0,0.0 +0,0.377700,0.0,0.0,0.0,0.0,0.0 +0,0.377800,0.0,0.0,0.0,0.0,0.0 +0,0.377900,0.0,0.0,0.0,0.0,0.0 +0,0.378000,0.0,0.0,0.0,0.0,0.0 +0,0.378100,0.0,0.0,0.0,0.0,0.0 +0,0.378200,0.0,0.0,0.0,0.0,0.0 +0,0.378300,0.0,0.0,0.0,0.0,0.0 +0,0.378400,0.0,0.0,0.0,0.0,0.0 +0,0.378500,0.0,0.0,0.0,0.0,0.0 +0,0.378600,0.0,0.0,0.0,0.0,0.0 +0,0.378700,0.0,0.0,0.0,0.0,0.0 +0,0.378800,0.0,0.0,0.0,0.0,0.0 +0,0.378900,0.0,0.0,0.0,0.0,0.0 +0,0.379000,0.0,0.0,0.0,0.0,0.0 +0,0.379100,0.0,0.0,0.0,0.0,0.0 +0,0.379200,0.0,0.0,0.0,0.0,0.0 +0,0.379300,0.0,0.0,0.0,0.0,0.0 +0,0.379400,0.0,0.0,0.0,0.0,0.0 +0,0.379500,0.0,0.0,0.0,0.0,0.0 +0,0.379600,0.0,0.0,0.0,0.0,0.0 +0,0.379700,0.0,0.0,0.0,0.0,0.0 +0,0.379800,0.0,0.0,0.0,0.0,0.0 +0,0.379900,0.0,0.0,0.0,0.0,0.0 +0,0.380000,0.0,0.0,0.0,0.0,0.0 +0,0.380100,0.0,0.0,0.0,0.0,0.0 +1,22.890419,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.380200,0.0,0.0,0.0,0.0,0.0 +0,0.380300,0.0,0.0,0.0,0.0,0.0 +0,0.380400,0.0,0.0,0.0,0.0,0.0 +0,0.380500,0.0,0.0,0.0,0.0,0.0 +0,0.380600,0.0,0.0,0.0,0.0,0.0 +0,0.380700,0.0,0.0,0.0,0.0,0.0 +0,0.380800,0.0,0.0,0.0,0.0,0.0 +0,0.380900,0.0,0.0,0.0,0.0,0.0 +0,0.381000,0.0,0.0,0.0,0.0,0.0 +0,0.381100,0.0,0.0,0.0,0.0,0.0 +0,0.381200,0.0,0.0,0.0,0.0,0.0 +0,0.381300,0.0,0.0,0.0,0.0,0.0 +0,0.381400,0.0,0.0,0.0,0.0,0.0 +0,0.381500,0.0,0.0,0.0,0.0,0.0 +0,0.381600,0.0,0.0,0.0,0.0,0.0 +0,0.381700,0.0,0.0,0.0,0.0,0.0 +0,0.381800,0.0,0.0,0.0,0.0,0.0 +0,0.381900,0.0,0.0,0.0,0.0,0.0 +0,0.382000,0.0,0.0,0.0,0.0,0.0 +0,0.382100,0.0,0.0,0.0,0.0,0.0 +0,0.382200,0.0,0.0,0.0,0.0,0.0 +0,0.382300,0.0,0.0,0.0,0.0,0.0 +0,0.382400,0.0,0.0,0.0,0.0,0.0 +0,0.382500,0.0,0.0,0.0,0.0,0.0 +0,0.382600,0.0,0.0,0.0,0.0,0.0 +0,0.382700,0.0,0.0,0.0,0.0,0.0 +0,0.382800,0.0,0.0,0.0,0.0,0.0 +0,0.382900,0.0,0.0,0.0,0.0,0.0 +0,0.383000,0.0,0.0,0.0,0.0,0.0 +0,0.383100,0.0,0.0,0.0,0.0,0.0 +0,0.383200,0.0,0.0,0.0,0.0,0.0 +0,0.383300,0.0,0.0,0.0,0.0,0.0 +0,0.383400,0.0,0.0,0.0,0.0,0.0 +0,0.383500,0.0,0.0,0.0,0.0,0.0 +0,0.383600,0.0,0.0,0.0,0.0,0.0 +0,0.383700,0.0,0.0,0.0,0.0,0.0 +0,0.383800,0.0,0.0,0.0,0.0,0.0 +0,0.383900,0.0,0.0,0.0,0.0,0.0 +0,0.384000,0.0,0.0,0.0,0.0,0.0 +0,0.384100,0.0,0.0,0.0,0.0,0.0 +0,0.384200,0.0,0.0,0.0,0.0,0.0 +0,0.384300,0.0,0.0,0.0,0.0,0.0 +0,0.384400,0.0,0.0,0.0,0.0,0.0 +0,0.384500,0.0,0.0,0.0,0.0,0.0 +0,0.384600,0.0,0.0,0.0,0.0,0.0 +0,0.384700,0.0,0.0,0.0,0.0,0.0 +0,0.384800,0.0,0.0,0.0,0.0,0.0 +0,0.384900,0.0,0.0,0.0,0.0,0.0 +0,0.385000,0.0,0.0,0.0,0.0,0.0 +0,0.385100,0.0,0.0,0.0,0.0,0.0 +0,0.385200,0.0,0.0,0.0,0.0,0.0 +0,0.385300,0.0,0.0,0.0,0.0,0.0 +0,0.385400,0.0,0.0,0.0,0.0,0.0 +0,0.385500,0.0,0.0,0.0,0.0,0.0 +0,0.385600,0.0,0.0,0.0,0.0,0.0 +0,0.385700,0.0,0.0,0.0,0.0,0.0 +0,0.385800,0.0,0.0,0.0,0.0,0.0 +0,0.385900,0.0,0.0,0.0,0.0,0.0 +0,0.386000,0.0,0.0,0.0,0.0,0.0 +0,0.386100,0.0,0.0,0.0,0.0,0.0 +0,0.386200,0.0,0.0,0.0,0.0,0.0 +0,0.386300,0.0,0.0,0.0,0.0,0.0 +0,0.386400,0.0,0.0,0.0,0.0,0.0 +0,0.386500,0.0,0.0,0.0,0.0,0.0 +0,0.386600,0.0,0.0,0.0,0.0,0.0 +0,0.386700,0.0,0.0,0.0,0.0,0.0 +0,0.386800,0.0,0.0,0.0,0.0,0.0 +0,0.386900,0.0,0.0,0.0,0.0,0.0 +0,0.387000,0.0,0.0,0.0,0.0,0.0 +0,0.387100,0.0,0.0,0.0,0.0,0.0 +0,0.387200,0.0,0.0,0.0,0.0,0.0 +0,0.387300,0.0,0.0,0.0,0.0,0.0 +0,0.387400,0.0,0.0,0.0,0.0,0.0 +0,0.387500,0.0,0.0,0.0,0.0,0.0 +0,0.387600,0.0,0.0,0.0,0.0,0.0 +0,0.387700,0.0,0.0,0.0,0.0,0.0 +0,0.387800,0.0,0.0,0.0,0.0,0.0 +0,0.387900,0.0,0.0,0.0,0.0,0.0 +0,0.388000,0.0,0.0,0.0,0.0,0.0 +0,0.388100,0.0,0.0,0.0,0.0,0.0 +0,0.388200,0.0,0.0,0.0,0.0,0.0 +0,0.388300,0.0,0.0,0.0,0.0,0.0 +0,0.388400,0.0,0.0,0.0,0.0,0.0 +0,0.388500,0.0,0.0,0.0,0.0,0.0 +0,0.388600,0.0,0.0,0.0,0.0,0.0 +0,0.388700,0.0,0.0,0.0,0.0,0.0 +0,0.388800,0.0,0.0,0.0,0.0,0.0 +0,0.388900,0.0,0.0,0.0,0.0,0.0 +0,0.389000,0.0,0.0,0.0,0.0,0.0 +0,0.389100,0.0,0.0,0.0,0.0,0.0 +0,0.389200,0.0,0.0,0.0,0.0,0.0 +0,0.389300,0.0,0.0,0.0,0.0,0.0 +0,0.389400,0.0,0.0,0.0,0.0,0.0 +0,0.389500,0.0,0.0,0.0,0.0,0.0 +0,0.389600,0.0,0.0,0.0,0.0,0.0 +0,0.389700,0.0,0.0,0.0,0.0,0.0 +0,0.389800,0.0,0.0,0.0,0.0,0.0 +0,0.389900,0.0,0.0,0.0,0.0,0.0 +0,0.390000,0.0,0.0,0.0,0.0,0.0 +0,0.390100,0.0,0.0,0.0,0.0,0.0 +0,0.390200,0.0,0.0,0.0,0.0,0.0 +0,0.390300,0.0,0.0,0.0,0.0,0.0 +0,0.390400,0.0,0.0,0.0,0.0,0.0 +0,0.390500,0.0,0.0,0.0,0.0,0.0 +0,0.390600,0.0,0.0,0.0,0.0,0.0 +0,0.390700,0.0,0.0,0.0,0.0,0.0 +0,0.390800,0.0,0.0,0.0,0.0,0.0 +0,0.390900,0.0,0.0,0.0,0.0,0.0 +0,0.391000,0.0,0.0,0.0,0.0,0.0 +0,0.391100,0.0,0.0,0.0,0.0,0.0 +0,0.391200,0.0,0.0,0.0,0.0,0.0 +0,0.391300,0.0,0.0,0.0,0.0,0.0 +0,0.391400,0.0,0.0,0.0,0.0,0.0 +0,0.391500,0.0,0.0,0.0,0.0,0.0 +0,0.391600,0.0,0.0,0.0,0.0,0.0 +0,0.391700,0.0,0.0,0.0,0.0,0.0 +0,0.391800,0.0,0.0,0.0,0.0,0.0 +0,0.391900,0.0,0.0,0.0,0.0,0.0 +0,0.392000,0.0,0.0,0.0,0.0,0.0 +0,0.392100,0.0,0.0,0.0,0.0,0.0 +0,0.392200,0.0,0.0,0.0,0.0,0.0 +0,0.392300,0.0,0.0,0.0,0.0,0.0 +0,0.392400,0.0,0.0,0.0,0.0,0.0 +0,0.392500,0.0,0.0,0.0,0.0,0.0 +0,0.392600,0.0,0.0,0.0,0.0,0.0 +0,0.392700,0.0,0.0,0.0,0.0,0.0 +0,0.392800,0.0,0.0,0.0,0.0,0.0 +0,0.392900,0.0,0.0,0.0,0.0,0.0 +0,0.393000,0.0,0.0,0.0,0.0,0.0 +0,0.393100,0.0,0.0,0.0,0.0,0.0 +0,0.393200,0.0,0.0,0.0,0.0,0.0 +0,0.393300,0.0,0.0,0.0,0.0,0.0 +0,0.393400,0.0,0.0,0.0,0.0,0.0 +0,0.393500,0.0,0.0,0.0,0.0,0.0 +0,0.393600,0.0,0.0,0.0,0.0,0.0 +0,0.393700,0.0,0.0,0.0,0.0,0.0 +0,0.393800,0.0,0.0,0.0,0.0,0.0 +0,0.393900,0.0,0.0,0.0,0.0,0.0 +0,0.394000,0.0,0.0,0.0,0.0,0.0 +0,0.394100,0.0,0.0,0.0,0.0,0.0 +0,0.394200,0.0,0.0,0.0,0.0,0.0 +0,0.394300,0.0,0.0,0.0,0.0,0.0 +0,0.394400,0.0,0.0,0.0,0.0,0.0 +0,0.394500,0.0,0.0,0.0,0.0,0.0 +0,0.394600,0.0,0.0,0.0,0.0,0.0 +0,0.394700,0.0,0.0,0.0,0.0,0.0 +0,0.394800,0.0,0.0,0.0,0.0,0.0 +0,0.394900,0.0,0.0,0.0,0.0,0.0 +0,0.395000,0.0,0.0,0.0,0.0,0.0 +0,0.395100,0.0,0.0,0.0,0.0,0.0 +0,0.395200,0.0,0.0,0.0,0.0,0.0 +0,0.395300,0.0,0.0,0.0,0.0,0.0 +0,0.395400,0.0,0.0,0.0,0.0,0.0 +0,0.395500,0.0,0.0,0.0,0.0,0.0 +0,0.395600,0.0,0.0,0.0,0.0,0.0 +0,0.395700,0.0,0.0,0.0,0.0,0.0 +0,0.395800,0.0,0.0,0.0,0.0,0.0 +0,0.395900,0.0,0.0,0.0,0.0,0.0 +0,0.396000,0.0,0.0,0.0,0.0,0.0 +0,0.396100,0.0,0.0,0.0,0.0,0.0 +0,0.396200,0.0,0.0,0.0,0.0,0.0 +0,0.396300,0.0,0.0,0.0,0.0,0.0 +0,0.396400,0.0,0.0,0.0,0.0,0.0 +0,0.396500,0.0,0.0,0.0,0.0,0.0 +0,0.396600,0.0,0.0,0.0,0.0,0.0 +0,0.396700,0.0,0.0,0.0,0.0,0.0 +0,0.396800,0.0,0.0,0.0,0.0,0.0 +0,0.396900,0.0,0.0,0.0,0.0,0.0 +0,0.397000,0.0,0.0,0.0,0.0,0.0 +0,0.397100,0.0,0.0,0.0,0.0,0.0 +0,0.397200,0.0,0.0,0.0,0.0,0.0 +0,0.397300,0.0,0.0,0.0,0.0,0.0 +0,0.397400,0.0,0.0,0.0,0.0,0.0 +0,0.397500,0.0,0.0,0.0,0.0,0.0 +0,0.397600,0.0,0.0,0.0,0.0,0.0 +0,0.397700,0.0,0.0,0.0,0.0,0.0 +0,0.397800,0.0,0.0,0.0,0.0,0.0 +0,0.397900,0.0,0.0,0.0,0.0,0.0 +0,0.398000,0.0,0.0,0.0,0.0,0.0 +0,0.398100,0.0,0.0,0.0,0.0,0.0 +0,0.398200,0.0,0.0,0.0,0.0,0.0 +0,0.398300,0.0,0.0,0.0,0.0,0.0 +0,0.398400,0.0,0.0,0.0,0.0,0.0 +0,0.398500,0.0,0.0,0.0,0.0,0.0 +0,0.398600,0.0,0.0,0.0,0.0,0.0 +0,0.398700,0.0,0.0,0.0,0.0,0.0 +0,0.398800,0.0,0.0,0.0,0.0,0.0 +0,0.398900,0.0,0.0,0.0,0.0,0.0 +0,0.399000,0.0,0.0,0.0,0.0,0.0 +0,0.399100,0.0,0.0,0.0,0.0,0.0 +0,0.399200,0.0,0.0,0.0,0.0,0.0 +0,0.399300,0.0,0.0,0.0,0.0,0.0 +0,0.399400,0.0,0.0,0.0,0.0,0.0 +0,0.399500,0.0,0.0,0.0,0.0,0.0 +0,0.399600,0.0,0.0,0.0,0.0,0.0 +0,0.399700,0.0,0.0,0.0,0.0,0.0 +0,0.399800,0.0,0.0,0.0,0.0,0.0 +0,0.399900,0.0,0.0,0.0,0.0,0.0 +0,0.400000,0.0,0.0,0.0,0.0,0.0 +0,0.400100,0.0,0.0,0.0,0.0,0.0 +1,26.696678,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.400200,0.0,0.0,0.0,0.0,0.0 +0,0.400300,0.0,0.0,0.0,0.0,0.0 +0,0.400400,0.0,0.0,0.0,0.0,0.0 +0,0.400500,0.0,0.0,0.0,0.0,0.0 +0,0.400600,0.0,0.0,0.0,0.0,0.0 +0,0.400700,0.0,0.0,0.0,0.0,0.0 +0,0.400800,0.0,0.0,0.0,0.0,0.0 +0,0.400900,0.0,0.0,0.0,0.0,0.0 +0,0.401000,0.0,0.0,0.0,0.0,0.0 +0,0.401100,0.0,0.0,0.0,0.0,0.0 +0,0.401200,0.0,0.0,0.0,0.0,0.0 +0,0.401300,0.0,0.0,0.0,0.0,0.0 +0,0.401400,0.0,0.0,0.0,0.0,0.0 +0,0.401500,0.0,0.0,0.0,0.0,0.0 +0,0.401600,0.0,0.0,0.0,0.0,0.0 +0,0.401700,0.0,0.0,0.0,0.0,0.0 +0,0.401800,0.0,0.0,0.0,0.0,0.0 +0,0.401900,0.0,0.0,0.0,0.0,0.0 +0,0.402000,0.0,0.0,0.0,0.0,0.0 +0,0.402100,0.0,0.0,0.0,0.0,0.0 +0,0.402200,0.0,0.0,0.0,0.0,0.0 +0,0.402300,0.0,0.0,0.0,0.0,0.0 +0,0.402400,0.0,0.0,0.0,0.0,0.0 +0,0.402500,0.0,0.0,0.0,0.0,0.0 +0,0.402600,0.0,0.0,0.0,0.0,0.0 +0,0.402700,0.0,0.0,0.0,0.0,0.0 +0,0.402800,0.0,0.0,0.0,0.0,0.0 +0,0.402900,0.0,0.0,0.0,0.0,0.0 +0,0.403000,0.0,0.0,0.0,0.0,0.0 +0,0.403100,0.0,0.0,0.0,0.0,0.0 +0,0.403200,0.0,0.0,0.0,0.0,0.0 +0,0.403300,0.0,0.0,0.0,0.0,0.0 +0,0.403400,0.0,0.0,0.0,0.0,0.0 +0,0.403500,0.0,0.0,0.0,0.0,0.0 +0,0.403600,0.0,0.0,0.0,0.0,0.0 +0,0.403700,0.0,0.0,0.0,0.0,0.0 +0,0.403800,0.0,0.0,0.0,0.0,0.0 +0,0.403900,0.0,0.0,0.0,0.0,0.0 +0,0.404000,0.0,0.0,0.0,0.0,0.0 +0,0.404100,0.0,0.0,0.0,0.0,0.0 +0,0.404200,0.0,0.0,0.0,0.0,0.0 +0,0.404300,0.0,0.0,0.0,0.0,0.0 +0,0.404400,0.0,0.0,0.0,0.0,0.0 +0,0.404500,0.0,0.0,0.0,0.0,0.0 +0,0.404600,0.0,0.0,0.0,0.0,0.0 +0,0.404700,0.0,0.0,0.0,0.0,0.0 +0,0.404800,0.0,0.0,0.0,0.0,0.0 +0,0.404900,0.0,0.0,0.0,0.0,0.0 +0,0.405000,0.0,0.0,0.0,0.0,0.0 +0,0.405100,0.0,0.0,0.0,0.0,0.0 +0,0.405200,0.0,0.0,0.0,0.0,0.0 +0,0.405300,0.0,0.0,0.0,0.0,0.0 +0,0.405400,0.0,0.0,0.0,0.0,0.0 +0,0.405500,0.0,0.0,0.0,0.0,0.0 +0,0.405600,0.0,0.0,0.0,0.0,0.0 +0,0.405700,0.0,0.0,0.0,0.0,0.0 +0,0.405800,0.0,0.0,0.0,0.0,0.0 +0,0.405900,0.0,0.0,0.0,0.0,0.0 +0,0.406000,0.0,0.0,0.0,0.0,0.0 +0,0.406100,0.0,0.0,0.0,0.0,0.0 +0,0.406200,0.0,0.0,0.0,0.0,0.0 +0,0.406300,0.0,0.0,0.0,0.0,0.0 +0,0.406400,0.0,0.0,0.0,0.0,0.0 +0,0.406500,0.0,0.0,0.0,0.0,0.0 +0,0.406600,0.0,0.0,0.0,0.0,0.0 +0,0.406700,0.0,0.0,0.0,0.0,0.0 +0,0.406800,0.0,0.0,0.0,0.0,0.0 +0,0.406900,0.0,0.0,0.0,0.0,0.0 +0,0.407000,0.0,0.0,0.0,0.0,0.0 +0,0.407100,0.0,0.0,0.0,0.0,0.0 +0,0.407200,0.0,0.0,0.0,0.0,0.0 +0,0.407300,0.0,0.0,0.0,0.0,0.0 +0,0.407400,0.0,0.0,0.0,0.0,0.0 +0,0.407500,0.0,0.0,0.0,0.0,0.0 +0,0.407600,0.0,0.0,0.0,0.0,0.0 +0,0.407700,0.0,0.0,0.0,0.0,0.0 +0,0.407800,0.0,0.0,0.0,0.0,0.0 +0,0.407900,0.0,0.0,0.0,0.0,0.0 +0,0.408000,0.0,0.0,0.0,0.0,0.0 +0,0.408100,0.0,0.0,0.0,0.0,0.0 +0,0.408200,0.0,0.0,0.0,0.0,0.0 +0,0.408300,0.0,0.0,0.0,0.0,0.0 +0,0.408400,0.0,0.0,0.0,0.0,0.0 +0,0.408500,0.0,0.0,0.0,0.0,0.0 +0,0.408600,0.0,0.0,0.0,0.0,0.0 +0,0.408700,0.0,0.0,0.0,0.0,0.0 +0,0.408800,0.0,0.0,0.0,0.0,0.0 +0,0.408900,0.0,0.0,0.0,0.0,0.0 +0,0.409000,0.0,0.0,0.0,0.0,0.0 +0,0.409100,0.0,0.0,0.0,0.0,0.0 +0,0.409200,0.0,0.0,0.0,0.0,0.0 +0,0.409300,0.0,0.0,0.0,0.0,0.0 +0,0.409400,0.0,0.0,0.0,0.0,0.0 +0,0.409500,0.0,0.0,0.0,0.0,0.0 +0,0.409600,0.0,0.0,0.0,0.0,0.0 +0,0.409700,0.0,0.0,0.0,0.0,0.0 +0,0.409800,0.0,0.0,0.0,0.0,0.0 +0,0.409900,0.0,0.0,0.0,0.0,0.0 +0,0.410000,0.0,0.0,0.0,0.0,0.0 +0,0.410100,0.0,0.0,0.0,0.0,0.0 +0,0.410200,0.0,0.0,0.0,0.0,0.0 +0,0.410300,0.0,0.0,0.0,0.0,0.0 +0,0.410400,0.0,0.0,0.0,0.0,0.0 +0,0.410500,0.0,0.0,0.0,0.0,0.0 +0,0.410600,0.0,0.0,0.0,0.0,0.0 +0,0.410700,0.0,0.0,0.0,0.0,0.0 +0,0.410800,0.0,0.0,0.0,0.0,0.0 +0,0.410900,0.0,0.0,0.0,0.0,0.0 +0,0.411000,0.0,0.0,0.0,0.0,0.0 +0,0.411100,0.0,0.0,0.0,0.0,0.0 +0,0.411200,0.0,0.0,0.0,0.0,0.0 +0,0.411300,0.0,0.0,0.0,0.0,0.0 +0,0.411400,0.0,0.0,0.0,0.0,0.0 +0,0.411500,0.0,0.0,0.0,0.0,0.0 +0,0.411600,0.0,0.0,0.0,0.0,0.0 +0,0.411700,0.0,0.0,0.0,0.0,0.0 +0,0.411800,0.0,0.0,0.0,0.0,0.0 +0,0.411900,0.0,0.0,0.0,0.0,0.0 +0,0.412000,0.0,0.0,0.0,0.0,0.0 +0,0.412100,0.0,0.0,0.0,0.0,0.0 +0,0.412200,0.0,0.0,0.0,0.0,0.0 +0,0.412300,0.0,0.0,0.0,0.0,0.0 +0,0.412400,0.0,0.0,0.0,0.0,0.0 +0,0.412500,0.0,0.0,0.0,0.0,0.0 +0,0.412600,0.0,0.0,0.0,0.0,0.0 +0,0.412700,0.0,0.0,0.0,0.0,0.0 +0,0.412800,0.0,0.0,0.0,0.0,0.0 +0,0.412900,0.0,0.0,0.0,0.0,0.0 +0,0.413000,0.0,0.0,0.0,0.0,0.0 +0,0.413100,0.0,0.0,0.0,0.0,0.0 +0,0.413200,0.0,0.0,0.0,0.0,0.0 +0,0.413300,0.0,0.0,0.0,0.0,0.0 +0,0.413400,0.0,0.0,0.0,0.0,0.0 +0,0.413500,0.0,0.0,0.0,0.0,0.0 +0,0.413600,0.0,0.0,0.0,0.0,0.0 +0,0.413700,0.0,0.0,0.0,0.0,0.0 +0,0.413800,0.0,0.0,0.0,0.0,0.0 +0,0.413900,0.0,0.0,0.0,0.0,0.0 +0,0.414000,0.0,0.0,0.0,0.0,0.0 +0,0.414100,0.0,0.0,0.0,0.0,0.0 +0,0.414200,0.0,0.0,0.0,0.0,0.0 +0,0.414300,0.0,0.0,0.0,0.0,0.0 +0,0.414400,0.0,0.0,0.0,0.0,0.0 +0,0.414500,0.0,0.0,0.0,0.0,0.0 +0,0.414600,0.0,0.0,0.0,0.0,0.0 +0,0.414700,0.0,0.0,0.0,0.0,0.0 +0,0.414800,0.0,0.0,0.0,0.0,0.0 +0,0.414900,0.0,0.0,0.0,0.0,0.0 +0,0.415000,0.0,0.0,0.0,0.0,0.0 +0,0.415100,0.0,0.0,0.0,0.0,0.0 +0,0.415200,0.0,0.0,0.0,0.0,0.0 +0,0.415300,0.0,0.0,0.0,0.0,0.0 +0,0.415400,0.0,0.0,0.0,0.0,0.0 +0,0.415500,0.0,0.0,0.0,0.0,0.0 +0,0.415600,0.0,0.0,0.0,0.0,0.0 +0,0.415700,0.0,0.0,0.0,0.0,0.0 +0,0.415800,0.0,0.0,0.0,0.0,0.0 +0,0.415900,0.0,0.0,0.0,0.0,0.0 +0,0.416000,0.0,0.0,0.0,0.0,0.0 +0,0.416100,0.0,0.0,0.0,0.0,0.0 +0,0.416200,0.0,0.0,0.0,0.0,0.0 +0,0.416300,0.0,0.0,0.0,0.0,0.0 +0,0.416400,0.0,0.0,0.0,0.0,0.0 +0,0.416500,0.0,0.0,0.0,0.0,0.0 +0,0.416600,0.0,0.0,0.0,0.0,0.0 +0,0.416700,0.0,0.0,0.0,0.0,0.0 +0,0.416800,0.0,0.0,0.0,0.0,0.0 +0,0.416900,0.0,0.0,0.0,0.0,0.0 +0,0.417000,0.0,0.0,0.0,0.0,0.0 +0,0.417100,0.0,0.0,0.0,0.0,0.0 +0,0.417200,0.0,0.0,0.0,0.0,0.0 +0,0.417300,0.0,0.0,0.0,0.0,0.0 +0,0.417400,0.0,0.0,0.0,0.0,0.0 +0,0.417500,0.0,0.0,0.0,0.0,0.0 +0,0.417600,0.0,0.0,0.0,0.0,0.0 +0,0.417700,0.0,0.0,0.0,0.0,0.0 +0,0.417800,0.0,0.0,0.0,0.0,0.0 +0,0.417900,0.0,0.0,0.0,0.0,0.0 +0,0.418000,0.0,0.0,0.0,0.0,0.0 +0,0.418100,0.0,0.0,0.0,0.0,0.0 +0,0.418200,0.0,0.0,0.0,0.0,0.0 +0,0.418300,0.0,0.0,0.0,0.0,0.0 +0,0.418400,0.0,0.0,0.0,0.0,0.0 +0,0.418500,0.0,0.0,0.0,0.0,0.0 +0,0.418600,0.0,0.0,0.0,0.0,0.0 +0,0.418700,0.0,0.0,0.0,0.0,0.0 +0,0.418800,0.0,0.0,0.0,0.0,0.0 +0,0.418900,0.0,0.0,0.0,0.0,0.0 +0,0.419000,0.0,0.0,0.0,0.0,0.0 +0,0.419100,0.0,0.0,0.0,0.0,0.0 +0,0.419200,0.0,0.0,0.0,0.0,0.0 +0,0.419300,0.0,0.0,0.0,0.0,0.0 +0,0.419400,0.0,0.0,0.0,0.0,0.0 +0,0.419500,0.0,0.0,0.0,0.0,0.0 +0,0.419600,0.0,0.0,0.0,0.0,0.0 +0,0.419700,0.0,0.0,0.0,0.0,0.0 +0,0.419800,0.0,0.0,0.0,0.0,0.0 +0,0.419900,0.0,0.0,0.0,0.0,0.0 +0,0.420000,0.0,0.0,0.0,0.0,0.0 +0,0.420100,0.0,0.0,0.0,0.0,0.0 +1,30.903086,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.420200,0.0,0.0,0.0,0.0,0.0 +0,0.420300,0.0,0.0,0.0,0.0,0.0 +0,0.420400,0.0,0.0,0.0,0.0,0.0 +0,0.420500,0.0,0.0,0.0,0.0,0.0 +0,0.420600,0.0,0.0,0.0,0.0,0.0 +0,0.420700,0.0,0.0,0.0,0.0,0.0 +0,0.420800,0.0,0.0,0.0,0.0,0.0 +0,0.420900,0.0,0.0,0.0,0.0,0.0 +0,0.421000,0.0,0.0,0.0,0.0,0.0 +0,0.421100,0.0,0.0,0.0,0.0,0.0 +0,0.421200,0.0,0.0,0.0,0.0,0.0 +0,0.421300,0.0,0.0,0.0,0.0,0.0 +0,0.421400,0.0,0.0,0.0,0.0,0.0 +0,0.421500,0.0,0.0,0.0,0.0,0.0 +0,0.421600,0.0,0.0,0.0,0.0,0.0 +0,0.421700,0.0,0.0,0.0,0.0,0.0 +0,0.421800,0.0,0.0,0.0,0.0,0.0 +0,0.421900,0.0,0.0,0.0,0.0,0.0 +0,0.422000,0.0,0.0,0.0,0.0,0.0 +0,0.422100,0.0,0.0,0.0,0.0,0.0 +0,0.422200,0.0,0.0,0.0,0.0,0.0 +0,0.422300,0.0,0.0,0.0,0.0,0.0 +0,0.422400,0.0,0.0,0.0,0.0,0.0 +0,0.422500,0.0,0.0,0.0,0.0,0.0 +0,0.422600,0.0,0.0,0.0,0.0,0.0 +0,0.422700,0.0,0.0,0.0,0.0,0.0 +0,0.422800,0.0,0.0,0.0,0.0,0.0 +0,0.422900,0.0,0.0,0.0,0.0,0.0 +0,0.423000,0.0,0.0,0.0,0.0,0.0 +0,0.423100,0.0,0.0,0.0,0.0,0.0 +0,0.423200,0.0,0.0,0.0,0.0,0.0 +0,0.423300,0.0,0.0,0.0,0.0,0.0 +0,0.423400,0.0,0.0,0.0,0.0,0.0 +0,0.423500,0.0,0.0,0.0,0.0,0.0 +0,0.423600,0.0,0.0,0.0,0.0,0.0 +0,0.423700,0.0,0.0,0.0,0.0,0.0 +0,0.423800,0.0,0.0,0.0,0.0,0.0 +0,0.423900,0.0,0.0,0.0,0.0,0.0 +0,0.424000,0.0,0.0,0.0,0.0,0.0 +0,0.424100,0.0,0.0,0.0,0.0,0.0 +0,0.424200,0.0,0.0,0.0,0.0,0.0 +0,0.424300,0.0,0.0,0.0,0.0,0.0 +0,0.424400,0.0,0.0,0.0,0.0,0.0 +0,0.424500,0.0,0.0,0.0,0.0,0.0 +0,0.424600,0.0,0.0,0.0,0.0,0.0 +0,0.424700,0.0,0.0,0.0,0.0,0.0 +0,0.424800,0.0,0.0,0.0,0.0,0.0 +0,0.424900,0.0,0.0,0.0,0.0,0.0 +0,0.425000,0.0,0.0,0.0,0.0,0.0 +0,0.425100,0.0,0.0,0.0,0.0,0.0 +0,0.425200,0.0,0.0,0.0,0.0,0.0 +0,0.425300,0.0,0.0,0.0,0.0,0.0 +0,0.425400,0.0,0.0,0.0,0.0,0.0 +0,0.425500,0.0,0.0,0.0,0.0,0.0 +0,0.425600,0.0,0.0,0.0,0.0,0.0 +0,0.425700,0.0,0.0,0.0,0.0,0.0 +0,0.425800,0.0,0.0,0.0,0.0,0.0 +0,0.425900,0.0,0.0,0.0,0.0,0.0 +0,0.426000,0.0,0.0,0.0,0.0,0.0 +0,0.426100,0.0,0.0,0.0,0.0,0.0 +0,0.426200,0.0,0.0,0.0,0.0,0.0 +0,0.426300,0.0,0.0,0.0,0.0,0.0 +0,0.426400,0.0,0.0,0.0,0.0,0.0 +0,0.426500,0.0,0.0,0.0,0.0,0.0 +0,0.426600,0.0,0.0,0.0,0.0,0.0 +0,0.426700,0.0,0.0,0.0,0.0,0.0 +0,0.426800,0.0,0.0,0.0,0.0,0.0 +0,0.426900,0.0,0.0,0.0,0.0,0.0 +0,0.427000,0.0,0.0,0.0,0.0,0.0 +0,0.427100,0.0,0.0,0.0,0.0,0.0 +0,0.427200,0.0,0.0,0.0,0.0,0.0 +0,0.427300,0.0,0.0,0.0,0.0,0.0 +0,0.427400,0.0,0.0,0.0,0.0,0.0 +0,0.427500,0.0,0.0,0.0,0.0,0.0 +0,0.427600,0.0,0.0,0.0,0.0,0.0 +0,0.427700,0.0,0.0,0.0,0.0,0.0 +0,0.427800,0.0,0.0,0.0,0.0,0.0 +0,0.427900,0.0,0.0,0.0,0.0,0.0 +0,0.428000,0.0,0.0,0.0,0.0,0.0 +0,0.428100,0.0,0.0,0.0,0.0,0.0 +0,0.428200,0.0,0.0,0.0,0.0,0.0 +0,0.428300,0.0,0.0,0.0,0.0,0.0 +0,0.428400,0.0,0.0,0.0,0.0,0.0 +0,0.428500,0.0,0.0,0.0,0.0,0.0 +0,0.428600,0.0,0.0,0.0,0.0,0.0 +0,0.428700,0.0,0.0,0.0,0.0,0.0 +0,0.428800,0.0,0.0,0.0,0.0,0.0 +0,0.428900,0.0,0.0,0.0,0.0,0.0 +0,0.429000,0.0,0.0,0.0,0.0,0.0 +0,0.429100,0.0,0.0,0.0,0.0,0.0 +0,0.429200,0.0,0.0,0.0,0.0,0.0 +0,0.429300,0.0,0.0,0.0,0.0,0.0 +0,0.429400,0.0,0.0,0.0,0.0,0.0 +0,0.429500,0.0,0.0,0.0,0.0,0.0 +0,0.429600,0.0,0.0,0.0,0.0,0.0 +0,0.429700,0.0,0.0,0.0,0.0,0.0 +0,0.429800,0.0,0.0,0.0,0.0,0.0 +0,0.429900,0.0,0.0,0.0,0.0,0.0 +0,0.430000,0.0,0.0,0.0,0.0,0.0 +0,0.430100,0.0,0.0,0.0,0.0,0.0 +0,0.430200,0.0,0.0,0.0,0.0,0.0 +0,0.430300,0.0,0.0,0.0,0.0,0.0 +0,0.430400,0.0,0.0,0.0,0.0,0.0 +0,0.430500,0.0,0.0,0.0,0.0,0.0 +0,0.430600,0.0,0.0,0.0,0.0,0.0 +0,0.430700,0.0,0.0,0.0,0.0,0.0 +0,0.430800,0.0,0.0,0.0,0.0,0.0 +0,0.430900,0.0,0.0,0.0,0.0,0.0 +0,0.431000,0.0,0.0,0.0,0.0,0.0 +0,0.431100,0.0,0.0,0.0,0.0,0.0 +0,0.431200,0.0,0.0,0.0,0.0,0.0 +0,0.431300,0.0,0.0,0.0,0.0,0.0 +0,0.431400,0.0,0.0,0.0,0.0,0.0 +0,0.431500,0.0,0.0,0.0,0.0,0.0 +0,0.431600,0.0,0.0,0.0,0.0,0.0 +0,0.431700,0.0,0.0,0.0,0.0,0.0 +0,0.431800,0.0,0.0,0.0,0.0,0.0 +0,0.431900,0.0,0.0,0.0,0.0,0.0 +0,0.432000,0.0,0.0,0.0,0.0,0.0 +0,0.432100,0.0,0.0,0.0,0.0,0.0 +0,0.432200,0.0,0.0,0.0,0.0,0.0 +0,0.432300,0.0,0.0,0.0,0.0,0.0 +0,0.432400,0.0,0.0,0.0,0.0,0.0 +0,0.432500,0.0,0.0,0.0,0.0,0.0 +0,0.432600,0.0,0.0,0.0,0.0,0.0 +0,0.432700,0.0,0.0,0.0,0.0,0.0 +0,0.432800,0.0,0.0,0.0,0.0,0.0 +0,0.432900,0.0,0.0,0.0,0.0,0.0 +0,0.433000,0.0,0.0,0.0,0.0,0.0 +0,0.433100,0.0,0.0,0.0,0.0,0.0 +0,0.433200,0.0,0.0,0.0,0.0,0.0 +0,0.433300,0.0,0.0,0.0,0.0,0.0 +0,0.433400,0.0,0.0,0.0,0.0,0.0 +0,0.433500,0.0,0.0,0.0,0.0,0.0 +0,0.433600,0.0,0.0,0.0,0.0,0.0 +0,0.433700,0.0,0.0,0.0,0.0,0.0 +0,0.433800,0.0,0.0,0.0,0.0,0.0 +0,0.433900,0.0,0.0,0.0,0.0,0.0 +0,0.434000,0.0,0.0,0.0,0.0,0.0 +0,0.434100,0.0,0.0,0.0,0.0,0.0 +0,0.434200,0.0,0.0,0.0,0.0,0.0 +0,0.434300,0.0,0.0,0.0,0.0,0.0 +0,0.434400,0.0,0.0,0.0,0.0,0.0 +0,0.434500,0.0,0.0,0.0,0.0,0.0 +0,0.434600,0.0,0.0,0.0,0.0,0.0 +0,0.434700,0.0,0.0,0.0,0.0,0.0 +0,0.434800,0.0,0.0,0.0,0.0,0.0 +0,0.434900,0.0,0.0,0.0,0.0,0.0 +0,0.435000,0.0,0.0,0.0,0.0,0.0 +0,0.435100,0.0,0.0,0.0,0.0,0.0 +0,0.435200,0.0,0.0,0.0,0.0,0.0 +0,0.435300,0.0,0.0,0.0,0.0,0.0 +0,0.435400,0.0,0.0,0.0,0.0,0.0 +0,0.435500,0.0,0.0,0.0,0.0,0.0 +0,0.435600,0.0,0.0,0.0,0.0,0.0 +0,0.435700,0.0,0.0,0.0,0.0,0.0 +0,0.435800,0.0,0.0,0.0,0.0,0.0 +0,0.435900,0.0,0.0,0.0,0.0,0.0 +0,0.436000,0.0,0.0,0.0,0.0,0.0 +0,0.436100,0.0,0.0,0.0,0.0,0.0 +0,0.436200,0.0,0.0,0.0,0.0,0.0 +0,0.436300,0.0,0.0,0.0,0.0,0.0 +0,0.436400,0.0,0.0,0.0,0.0,0.0 +0,0.436500,0.0,0.0,0.0,0.0,0.0 +0,0.436600,0.0,0.0,0.0,0.0,0.0 +0,0.436700,0.0,0.0,0.0,0.0,0.0 +0,0.436800,0.0,0.0,0.0,0.0,0.0 +0,0.436900,0.0,0.0,0.0,0.0,0.0 +0,0.437000,0.0,0.0,0.0,0.0,0.0 +0,0.437100,0.0,0.0,0.0,0.0,0.0 +0,0.437200,0.0,0.0,0.0,0.0,0.0 +0,0.437300,0.0,0.0,0.0,0.0,0.0 +0,0.437400,0.0,0.0,0.0,0.0,0.0 +0,0.437500,0.0,0.0,0.0,0.0,0.0 +0,0.437600,0.0,0.0,0.0,0.0,0.0 +0,0.437700,0.0,0.0,0.0,0.0,0.0 +0,0.437800,0.0,0.0,0.0,0.0,0.0 +0,0.437900,0.0,0.0,0.0,0.0,0.0 +0,0.438000,0.0,0.0,0.0,0.0,0.0 +0,0.438100,0.0,0.0,0.0,0.0,0.0 +0,0.438200,0.0,0.0,0.0,0.0,0.0 +0,0.438300,0.0,0.0,0.0,0.0,0.0 +0,0.438400,0.0,0.0,0.0,0.0,0.0 +0,0.438500,0.0,0.0,0.0,0.0,0.0 +0,0.438600,0.0,0.0,0.0,0.0,0.0 +0,0.438700,0.0,0.0,0.0,0.0,0.0 +0,0.438800,0.0,0.0,0.0,0.0,0.0 +0,0.438900,0.0,0.0,0.0,0.0,0.0 +0,0.439000,0.0,0.0,0.0,0.0,0.0 +0,0.439100,0.0,0.0,0.0,0.0,0.0 +0,0.439200,0.0,0.0,0.0,0.0,0.0 +0,0.439300,0.0,0.0,0.0,0.0,0.0 +0,0.439400,0.0,0.0,0.0,0.0,0.0 +0,0.439500,0.0,0.0,0.0,0.0,0.0 +0,0.439600,0.0,0.0,0.0,0.0,0.0 +0,0.439700,0.0,0.0,0.0,0.0,0.0 +0,0.439800,0.0,0.0,0.0,0.0,0.0 +0,0.439900,0.0,0.0,0.0,0.0,0.0 +0,0.440000,0.0,0.0,0.0,0.0,0.0 +0,0.440100,0.0,0.0,0.0,0.0,0.0 +1,35.529645,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.440200,0.0,0.0,0.0,0.0,0.0 +0,0.440300,0.0,0.0,0.0,0.0,0.0 +0,0.440400,0.0,0.0,0.0,0.0,0.0 +0,0.440500,0.0,0.0,0.0,0.0,0.0 +0,0.440600,0.0,0.0,0.0,0.0,0.0 +0,0.440700,0.0,0.0,0.0,0.0,0.0 +0,0.440800,0.0,0.0,0.0,0.0,0.0 +0,0.440900,0.0,0.0,0.0,0.0,0.0 +0,0.441000,0.0,0.0,0.0,0.0,0.0 +0,0.441100,0.0,0.0,0.0,0.0,0.0 +0,0.441200,0.0,0.0,0.0,0.0,0.0 +0,0.441300,0.0,0.0,0.0,0.0,0.0 +0,0.441400,0.0,0.0,0.0,0.0,0.0 +0,0.441500,0.0,0.0,0.0,0.0,0.0 +0,0.441600,0.0,0.0,0.0,0.0,0.0 +0,0.441700,0.0,0.0,0.0,0.0,0.0 +0,0.441800,0.0,0.0,0.0,0.0,0.0 +0,0.441900,0.0,0.0,0.0,0.0,0.0 +0,0.442000,0.0,0.0,0.0,0.0,0.0 +0,0.442100,0.0,0.0,0.0,0.0,0.0 +0,0.442200,0.0,0.0,0.0,0.0,0.0 +0,0.442300,0.0,0.0,0.0,0.0,0.0 +0,0.442400,0.0,0.0,0.0,0.0,0.0 +0,0.442500,0.0,0.0,0.0,0.0,0.0 +0,0.442600,0.0,0.0,0.0,0.0,0.0 +0,0.442700,0.0,0.0,0.0,0.0,0.0 +0,0.442800,0.0,0.0,0.0,0.0,0.0 +0,0.442900,0.0,0.0,0.0,0.0,0.0 +0,0.443000,0.0,0.0,0.0,0.0,0.0 +0,0.443100,0.0,0.0,0.0,0.0,0.0 +0,0.443200,0.0,0.0,0.0,0.0,0.0 +0,0.443300,0.0,0.0,0.0,0.0,0.0 +0,0.443400,0.0,0.0,0.0,0.0,0.0 +0,0.443500,0.0,0.0,0.0,0.0,0.0 +0,0.443600,0.0,0.0,0.0,0.0,0.0 +0,0.443700,0.0,0.0,0.0,0.0,0.0 +0,0.443800,0.0,0.0,0.0,0.0,0.0 +0,0.443900,0.0,0.0,0.0,0.0,0.0 +0,0.444000,0.0,0.0,0.0,0.0,0.0 +0,0.444100,0.0,0.0,0.0,0.0,0.0 +0,0.444200,0.0,0.0,0.0,0.0,0.0 +0,0.444300,0.0,0.0,0.0,0.0,0.0 +0,0.444400,0.0,0.0,0.0,0.0,0.0 +0,0.444500,0.0,0.0,0.0,0.0,0.0 +0,0.444600,0.0,0.0,0.0,0.0,0.0 +0,0.444700,0.0,0.0,0.0,0.0,0.0 +0,0.444800,0.0,0.0,0.0,0.0,0.0 +0,0.444900,0.0,0.0,0.0,0.0,0.0 +0,0.445000,0.0,0.0,0.0,0.0,0.0 +0,0.445100,0.0,0.0,0.0,0.0,0.0 +0,0.445200,0.0,0.0,0.0,0.0,0.0 +0,0.445300,0.0,0.0,0.0,0.0,0.0 +0,0.445400,0.0,0.0,0.0,0.0,0.0 +0,0.445500,0.0,0.0,0.0,0.0,0.0 +0,0.445600,0.0,0.0,0.0,0.0,0.0 +0,0.445700,0.0,0.0,0.0,0.0,0.0 +0,0.445800,0.0,0.0,0.0,0.0,0.0 +0,0.445900,0.0,0.0,0.0,0.0,0.0 +0,0.446000,0.0,0.0,0.0,0.0,0.0 +0,0.446100,0.0,0.0,0.0,0.0,0.0 +0,0.446200,0.0,0.0,0.0,0.0,0.0 +0,0.446300,0.0,0.0,0.0,0.0,0.0 +0,0.446400,0.0,0.0,0.0,0.0,0.0 +0,0.446500,0.0,0.0,0.0,0.0,0.0 +0,0.446600,0.0,0.0,0.0,0.0,0.0 +0,0.446700,0.0,0.0,0.0,0.0,0.0 +0,0.446800,0.0,0.0,0.0,0.0,0.0 +0,0.446900,0.0,0.0,0.0,0.0,0.0 +0,0.447000,0.0,0.0,0.0,0.0,0.0 +0,0.447100,0.0,0.0,0.0,0.0,0.0 +0,0.447200,0.0,0.0,0.0,0.0,0.0 +0,0.447300,0.0,0.0,0.0,0.0,0.0 +0,0.447400,0.0,0.0,0.0,0.0,0.0 +0,0.447500,0.0,0.0,0.0,0.0,0.0 +0,0.447600,0.0,0.0,0.0,0.0,0.0 +0,0.447700,0.0,0.0,0.0,0.0,0.0 +0,0.447800,0.0,0.0,0.0,0.0,0.0 +0,0.447900,0.0,0.0,0.0,0.0,0.0 +0,0.448000,0.0,0.0,0.0,0.0,0.0 +0,0.448100,0.0,0.0,0.0,0.0,0.0 +0,0.448200,0.0,0.0,0.0,0.0,0.0 +0,0.448300,0.0,0.0,0.0,0.0,0.0 +0,0.448400,0.0,0.0,0.0,0.0,0.0 +0,0.448500,0.0,0.0,0.0,0.0,0.0 +0,0.448600,0.0,0.0,0.0,0.0,0.0 +0,0.448700,0.0,0.0,0.0,0.0,0.0 +0,0.448800,0.0,0.0,0.0,0.0,0.0 +0,0.448900,0.0,0.0,0.0,0.0,0.0 +0,0.449000,0.0,0.0,0.0,0.0,0.0 +0,0.449100,0.0,0.0,0.0,0.0,0.0 +0,0.449200,0.0,0.0,0.0,0.0,0.0 +0,0.449300,0.0,0.0,0.0,0.0,0.0 +0,0.449400,0.0,0.0,0.0,0.0,0.0 +0,0.449500,0.0,0.0,0.0,0.0,0.0 +0,0.449600,0.0,0.0,0.0,0.0,0.0 +0,0.449700,0.0,0.0,0.0,0.0,0.0 +0,0.449800,0.0,0.0,0.0,0.0,0.0 +0,0.449900,0.0,0.0,0.0,0.0,0.0 +0,0.450000,0.0,0.0,0.0,0.0,0.0 +0,0.450100,0.0,0.0,0.0,0.0,0.0 +0,0.450200,0.0,0.0,0.0,0.0,0.0 +0,0.450300,0.0,0.0,0.0,0.0,0.0 +0,0.450400,0.0,0.0,0.0,0.0,0.0 +0,0.450500,0.0,0.0,0.0,0.0,0.0 +0,0.450600,0.0,0.0,0.0,0.0,0.0 +0,0.450700,0.0,0.0,0.0,0.0,0.0 +0,0.450800,0.0,0.0,0.0,0.0,0.0 +0,0.450900,0.0,0.0,0.0,0.0,0.0 +0,0.451000,0.0,0.0,0.0,0.0,0.0 +0,0.451100,0.0,0.0,0.0,0.0,0.0 +0,0.451200,0.0,0.0,0.0,0.0,0.0 +0,0.451300,0.0,0.0,0.0,0.0,0.0 +0,0.451400,0.0,0.0,0.0,0.0,0.0 +0,0.451500,0.0,0.0,0.0,0.0,0.0 +0,0.451600,0.0,0.0,0.0,0.0,0.0 +0,0.451700,0.0,0.0,0.0,0.0,0.0 +0,0.451800,0.0,0.0,0.0,0.0,0.0 +0,0.451900,0.0,0.0,0.0,0.0,0.0 +0,0.452000,0.0,0.0,0.0,0.0,0.0 +0,0.452100,0.0,0.0,0.0,0.0,0.0 +0,0.452200,0.0,0.0,0.0,0.0,0.0 +0,0.452300,0.0,0.0,0.0,0.0,0.0 +0,0.452400,0.0,0.0,0.0,0.0,0.0 +0,0.452500,0.0,0.0,0.0,0.0,0.0 +0,0.452600,0.0,0.0,0.0,0.0,0.0 +0,0.452700,0.0,0.0,0.0,0.0,0.0 +0,0.452800,0.0,0.0,0.0,0.0,0.0 +0,0.452900,0.0,0.0,0.0,0.0,0.0 +0,0.453000,0.0,0.0,0.0,0.0,0.0 +0,0.453100,0.0,0.0,0.0,0.0,0.0 +0,0.453200,0.0,0.0,0.0,0.0,0.0 +0,0.453300,0.0,0.0,0.0,0.0,0.0 +0,0.453400,0.0,0.0,0.0,0.0,0.0 +0,0.453500,0.0,0.0,0.0,0.0,0.0 +0,0.453600,0.0,0.0,0.0,0.0,0.0 +0,0.453700,0.0,0.0,0.0,0.0,0.0 +0,0.453800,0.0,0.0,0.0,0.0,0.0 +0,0.453900,0.0,0.0,0.0,0.0,0.0 +0,0.454000,0.0,0.0,0.0,0.0,0.0 +0,0.454100,0.0,0.0,0.0,0.0,0.0 +0,0.454200,0.0,0.0,0.0,0.0,0.0 +0,0.454300,0.0,0.0,0.0,0.0,0.0 +0,0.454400,0.0,0.0,0.0,0.0,0.0 +0,0.454500,0.0,0.0,0.0,0.0,0.0 +0,0.454600,0.0,0.0,0.0,0.0,0.0 +0,0.454700,0.0,0.0,0.0,0.0,0.0 +0,0.454800,0.0,0.0,0.0,0.0,0.0 +0,0.454900,0.0,0.0,0.0,0.0,0.0 +0,0.455000,0.0,0.0,0.0,0.0,0.0 +0,0.455100,0.0,0.0,0.0,0.0,0.0 +0,0.455200,0.0,0.0,0.0,0.0,0.0 +0,0.455300,0.0,0.0,0.0,0.0,0.0 +0,0.455400,0.0,0.0,0.0,0.0,0.0 +0,0.455500,0.0,0.0,0.0,0.0,0.0 +0,0.455600,0.0,0.0,0.0,0.0,0.0 +0,0.455700,0.0,0.0,0.0,0.0,0.0 +0,0.455800,0.0,0.0,0.0,0.0,0.0 +0,0.455900,0.0,0.0,0.0,0.0,0.0 +0,0.456000,0.0,0.0,0.0,0.0,0.0 +0,0.456100,0.0,0.0,0.0,0.0,0.0 +0,0.456200,0.0,0.0,0.0,0.0,0.0 +0,0.456300,0.0,0.0,0.0,0.0,0.0 +0,0.456400,0.0,0.0,0.0,0.0,0.0 +0,0.456500,0.0,0.0,0.0,0.0,0.0 +0,0.456600,0.0,0.0,0.0,0.0,0.0 +0,0.456700,0.0,0.0,0.0,0.0,0.0 +0,0.456800,0.0,0.0,0.0,0.0,0.0 +0,0.456900,0.0,0.0,0.0,0.0,0.0 +0,0.457000,0.0,0.0,0.0,0.0,0.0 +0,0.457100,0.0,0.0,0.0,0.0,0.0 +0,0.457200,0.0,0.0,0.0,0.0,0.0 +0,0.457300,0.0,0.0,0.0,0.0,0.0 +0,0.457400,0.0,0.0,0.0,0.0,0.0 +0,0.457500,0.0,0.0,0.0,0.0,0.0 +0,0.457600,0.0,0.0,0.0,0.0,0.0 +0,0.457700,0.0,0.0,0.0,0.0,0.0 +0,0.457800,0.0,0.0,0.0,0.0,0.0 +0,0.457900,0.0,0.0,0.0,0.0,0.0 +0,0.458000,0.0,0.0,0.0,0.0,0.0 +0,0.458100,0.0,0.0,0.0,0.0,0.0 +0,0.458200,0.0,0.0,0.0,0.0,0.0 +0,0.458300,0.0,0.0,0.0,0.0,0.0 +0,0.458400,0.0,0.0,0.0,0.0,0.0 +0,0.458500,0.0,0.0,0.0,0.0,0.0 +0,0.458600,0.0,0.0,0.0,0.0,0.0 +0,0.458700,0.0,0.0,0.0,0.0,0.0 +0,0.458800,0.0,0.0,0.0,0.0,0.0 +0,0.458900,0.0,0.0,0.0,0.0,0.0 +0,0.459000,0.0,0.0,0.0,0.0,0.0 +0,0.459100,0.0,0.0,0.0,0.0,0.0 +0,0.459200,0.0,0.0,0.0,0.0,0.0 +0,0.459300,0.0,0.0,0.0,0.0,0.0 +0,0.459400,0.0,0.0,0.0,0.0,0.0 +0,0.459500,0.0,0.0,0.0,0.0,0.0 +0,0.459600,0.0,0.0,0.0,0.0,0.0 +0,0.459700,0.0,0.0,0.0,0.0,0.0 +0,0.459800,0.0,0.0,0.0,0.0,0.0 +0,0.459900,0.0,0.0,0.0,0.0,0.0 +0,0.460000,0.0,0.0,0.0,0.0,0.0 +0,0.460100,0.0,0.0,0.0,0.0,0.0 +1,40.596354,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.460200,0.0,0.0,0.0,0.0,0.0 +0,0.460300,0.0,0.0,0.0,0.0,0.0 +0,0.460400,0.0,0.0,0.0,0.0,0.0 +0,0.460500,0.0,0.0,0.0,0.0,0.0 +0,0.460600,0.0,0.0,0.0,0.0,0.0 +0,0.460700,0.0,0.0,0.0,0.0,0.0 +0,0.460800,0.0,0.0,0.0,0.0,0.0 +0,0.460900,0.0,0.0,0.0,0.0,0.0 +0,0.461000,0.0,0.0,0.0,0.0,0.0 +0,0.461100,0.0,0.0,0.0,0.0,0.0 +0,0.461200,0.0,0.0,0.0,0.0,0.0 +0,0.461300,0.0,0.0,0.0,0.0,0.0 +0,0.461400,0.0,0.0,0.0,0.0,0.0 +0,0.461500,0.0,0.0,0.0,0.0,0.0 +0,0.461600,0.0,0.0,0.0,0.0,0.0 +0,0.461700,0.0,0.0,0.0,0.0,0.0 +0,0.461800,0.0,0.0,0.0,0.0,0.0 +0,0.461900,0.0,0.0,0.0,0.0,0.0 +0,0.462000,0.0,0.0,0.0,0.0,0.0 +0,0.462100,0.0,0.0,0.0,0.0,0.0 +0,0.462200,0.0,0.0,0.0,0.0,0.0 +0,0.462300,0.0,0.0,0.0,0.0,0.0 +0,0.462400,0.0,0.0,0.0,0.0,0.0 +0,0.462500,0.0,0.0,0.0,0.0,0.0 +0,0.462600,0.0,0.0,0.0,0.0,0.0 +0,0.462700,0.0,0.0,0.0,0.0,0.0 +0,0.462800,0.0,0.0,0.0,0.0,0.0 +0,0.462900,0.0,0.0,0.0,0.0,0.0 +0,0.463000,0.0,0.0,0.0,0.0,0.0 +0,0.463100,0.0,0.0,0.0,0.0,0.0 +0,0.463200,0.0,0.0,0.0,0.0,0.0 +0,0.463300,0.0,0.0,0.0,0.0,0.0 +0,0.463400,0.0,0.0,0.0,0.0,0.0 +0,0.463500,0.0,0.0,0.0,0.0,0.0 +0,0.463600,0.0,0.0,0.0,0.0,0.0 +0,0.463700,0.0,0.0,0.0,0.0,0.0 +0,0.463800,0.0,0.0,0.0,0.0,0.0 +0,0.463900,0.0,0.0,0.0,0.0,0.0 +0,0.464000,0.0,0.0,0.0,0.0,0.0 +0,0.464100,0.0,0.0,0.0,0.0,0.0 +0,0.464200,0.0,0.0,0.0,0.0,0.0 +0,0.464300,0.0,0.0,0.0,0.0,0.0 +0,0.464400,0.0,0.0,0.0,0.0,0.0 +0,0.464500,0.0,0.0,0.0,0.0,0.0 +0,0.464600,0.0,0.0,0.0,0.0,0.0 +0,0.464700,0.0,0.0,0.0,0.0,0.0 +0,0.464800,0.0,0.0,0.0,0.0,0.0 +0,0.464900,0.0,0.0,0.0,0.0,0.0 +0,0.465000,0.0,0.0,0.0,0.0,0.0 +0,0.465100,0.0,0.0,0.0,0.0,0.0 +0,0.465200,0.0,0.0,0.0,0.0,0.0 +0,0.465300,0.0,0.0,0.0,0.0,0.0 +0,0.465400,0.0,0.0,0.0,0.0,0.0 +0,0.465500,0.0,0.0,0.0,0.0,0.0 +0,0.465600,0.0,0.0,0.0,0.0,0.0 +0,0.465700,0.0,0.0,0.0,0.0,0.0 +0,0.465800,0.0,0.0,0.0,0.0,0.0 +0,0.465900,0.0,0.0,0.0,0.0,0.0 +0,0.466000,0.0,0.0,0.0,0.0,0.0 +0,0.466100,0.0,0.0,0.0,0.0,0.0 +0,0.466200,0.0,0.0,0.0,0.0,0.0 +0,0.466300,0.0,0.0,0.0,0.0,0.0 +0,0.466400,0.0,0.0,0.0,0.0,0.0 +0,0.466500,0.0,0.0,0.0,0.0,0.0 +0,0.466600,0.0,0.0,0.0,0.0,0.0 +0,0.466700,0.0,0.0,0.0,0.0,0.0 +0,0.466800,0.0,0.0,0.0,0.0,0.0 +0,0.466900,0.0,0.0,0.0,0.0,0.0 +0,0.467000,0.0,0.0,0.0,0.0,0.0 +0,0.467100,0.0,0.0,0.0,0.0,0.0 +0,0.467200,0.0,0.0,0.0,0.0,0.0 +0,0.467300,0.0,0.0,0.0,0.0,0.0 +0,0.467400,0.0,0.0,0.0,0.0,0.0 +0,0.467500,0.0,0.0,0.0,0.0,0.0 +0,0.467600,0.0,0.0,0.0,0.0,0.0 +0,0.467700,0.0,0.0,0.0,0.0,0.0 +0,0.467800,0.0,0.0,0.0,0.0,0.0 +0,0.467900,0.0,0.0,0.0,0.0,0.0 +0,0.468000,0.0,0.0,0.0,0.0,0.0 +0,0.468100,0.0,0.0,0.0,0.0,0.0 +0,0.468200,0.0,0.0,0.0,0.0,0.0 +0,0.468300,0.0,0.0,0.0,0.0,0.0 +0,0.468400,0.0,0.0,0.0,0.0,0.0 +0,0.468500,0.0,0.0,0.0,0.0,0.0 +0,0.468600,0.0,0.0,0.0,0.0,0.0 +0,0.468700,0.0,0.0,0.0,0.0,0.0 +0,0.468800,0.0,0.0,0.0,0.0,0.0 +0,0.468900,0.0,0.0,0.0,0.0,0.0 +0,0.469000,0.0,0.0,0.0,0.0,0.0 +0,0.469100,0.0,0.0,0.0,0.0,0.0 +0,0.469200,0.0,0.0,0.0,0.0,0.0 +0,0.469300,0.0,0.0,0.0,0.0,0.0 +0,0.469400,0.0,0.0,0.0,0.0,0.0 +0,0.469500,0.0,0.0,0.0,0.0,0.0 +0,0.469600,0.0,0.0,0.0,0.0,0.0 +0,0.469700,0.0,0.0,0.0,0.0,0.0 +0,0.469800,0.0,0.0,0.0,0.0,0.0 +0,0.469900,0.0,0.0,0.0,0.0,0.0 +0,0.470000,0.0,0.0,0.0,0.0,0.0 +0,0.470100,0.0,0.0,0.0,0.0,0.0 +0,0.470200,0.0,0.0,0.0,0.0,0.0 +0,0.470300,0.0,0.0,0.0,0.0,0.0 +0,0.470400,0.0,0.0,0.0,0.0,0.0 +0,0.470500,0.0,0.0,0.0,0.0,0.0 +0,0.470600,0.0,0.0,0.0,0.0,0.0 +0,0.470700,0.0,0.0,0.0,0.0,0.0 +0,0.470800,0.0,0.0,0.0,0.0,0.0 +0,0.470900,0.0,0.0,0.0,0.0,0.0 +0,0.471000,0.0,0.0,0.0,0.0,0.0 +0,0.471100,0.0,0.0,0.0,0.0,0.0 +0,0.471200,0.0,0.0,0.0,0.0,0.0 +0,0.471300,0.0,0.0,0.0,0.0,0.0 +0,0.471400,0.0,0.0,0.0,0.0,0.0 +0,0.471500,0.0,0.0,0.0,0.0,0.0 +0,0.471600,0.0,0.0,0.0,0.0,0.0 +0,0.471700,0.0,0.0,0.0,0.0,0.0 +0,0.471800,0.0,0.0,0.0,0.0,0.0 +0,0.471900,0.0,0.0,0.0,0.0,0.0 +0,0.472000,0.0,0.0,0.0,0.0,0.0 +0,0.472100,0.0,0.0,0.0,0.0,0.0 +0,0.472200,0.0,0.0,0.0,0.0,0.0 +0,0.472300,0.0,0.0,0.0,0.0,0.0 +0,0.472400,0.0,0.0,0.0,0.0,0.0 +0,0.472500,0.0,0.0,0.0,0.0,0.0 +0,0.472600,0.0,0.0,0.0,0.0,0.0 +0,0.472700,0.0,0.0,0.0,0.0,0.0 +0,0.472800,0.0,0.0,0.0,0.0,0.0 +0,0.472900,0.0,0.0,0.0,0.0,0.0 +0,0.473000,0.0,0.0,0.0,0.0,0.0 +0,0.473100,0.0,0.0,0.0,0.0,0.0 +0,0.473200,0.0,0.0,0.0,0.0,0.0 +0,0.473300,0.0,0.0,0.0,0.0,0.0 +0,0.473400,0.0,0.0,0.0,0.0,0.0 +0,0.473500,0.0,0.0,0.0,0.0,0.0 +0,0.473600,0.0,0.0,0.0,0.0,0.0 +0,0.473700,0.0,0.0,0.0,0.0,0.0 +0,0.473800,0.0,0.0,0.0,0.0,0.0 +0,0.473900,0.0,0.0,0.0,0.0,0.0 +0,0.474000,0.0,0.0,0.0,0.0,0.0 +0,0.474100,0.0,0.0,0.0,0.0,0.0 +0,0.474200,0.0,0.0,0.0,0.0,0.0 +0,0.474300,0.0,0.0,0.0,0.0,0.0 +0,0.474400,0.0,0.0,0.0,0.0,0.0 +0,0.474500,0.0,0.0,0.0,0.0,0.0 +0,0.474600,0.0,0.0,0.0,0.0,0.0 +0,0.474700,0.0,0.0,0.0,0.0,0.0 +0,0.474800,0.0,0.0,0.0,0.0,0.0 +0,0.474900,0.0,0.0,0.0,0.0,0.0 +0,0.475000,0.0,0.0,0.0,0.0,0.0 +0,0.475100,0.0,0.0,0.0,0.0,0.0 +0,0.475200,0.0,0.0,0.0,0.0,0.0 +0,0.475300,0.0,0.0,0.0,0.0,0.0 +0,0.475400,0.0,0.0,0.0,0.0,0.0 +0,0.475500,0.0,0.0,0.0,0.0,0.0 +0,0.475600,0.0,0.0,0.0,0.0,0.0 +0,0.475700,0.0,0.0,0.0,0.0,0.0 +0,0.475800,0.0,0.0,0.0,0.0,0.0 +0,0.475900,0.0,0.0,0.0,0.0,0.0 +0,0.476000,0.0,0.0,0.0,0.0,0.0 +0,0.476100,0.0,0.0,0.0,0.0,0.0 +0,0.476200,0.0,0.0,0.0,0.0,0.0 +0,0.476300,0.0,0.0,0.0,0.0,0.0 +0,0.476400,0.0,0.0,0.0,0.0,0.0 +0,0.476500,0.0,0.0,0.0,0.0,0.0 +0,0.476600,0.0,0.0,0.0,0.0,0.0 +0,0.476700,0.0,0.0,0.0,0.0,0.0 +0,0.476800,0.0,0.0,0.0,0.0,0.0 +0,0.476900,0.0,0.0,0.0,0.0,0.0 +0,0.477000,0.0,0.0,0.0,0.0,0.0 +0,0.477100,0.0,0.0,0.0,0.0,0.0 +0,0.477200,0.0,0.0,0.0,0.0,0.0 +0,0.477300,0.0,0.0,0.0,0.0,0.0 +0,0.477400,0.0,0.0,0.0,0.0,0.0 +0,0.477500,0.0,0.0,0.0,0.0,0.0 +0,0.477600,0.0,0.0,0.0,0.0,0.0 +0,0.477700,0.0,0.0,0.0,0.0,0.0 +0,0.477800,0.0,0.0,0.0,0.0,0.0 +0,0.477900,0.0,0.0,0.0,0.0,0.0 +0,0.478000,0.0,0.0,0.0,0.0,0.0 +0,0.478100,0.0,0.0,0.0,0.0,0.0 +0,0.478200,0.0,0.0,0.0,0.0,0.0 +0,0.478300,0.0,0.0,0.0,0.0,0.0 +0,0.478400,0.0,0.0,0.0,0.0,0.0 +0,0.478500,0.0,0.0,0.0,0.0,0.0 +0,0.478600,0.0,0.0,0.0,0.0,0.0 +0,0.478700,0.0,0.0,0.0,0.0,0.0 +0,0.478800,0.0,0.0,0.0,0.0,0.0 +0,0.478900,0.0,0.0,0.0,0.0,0.0 +0,0.479000,0.0,0.0,0.0,0.0,0.0 +0,0.479100,0.0,0.0,0.0,0.0,0.0 +0,0.479200,0.0,0.0,0.0,0.0,0.0 +0,0.479300,0.0,0.0,0.0,0.0,0.0 +0,0.479400,0.0,0.0,0.0,0.0,0.0 +0,0.479500,0.0,0.0,0.0,0.0,0.0 +0,0.479600,0.0,0.0,0.0,0.0,0.0 +0,0.479700,0.0,0.0,0.0,0.0,0.0 +0,0.479800,0.0,0.0,0.0,0.0,0.0 +0,0.479900,0.0,0.0,0.0,0.0,0.0 +0,0.480000,0.0,0.0,0.0,0.0,0.0 +0,0.480100,0.0,0.0,0.0,0.0,0.0 +1,46.123213,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.480200,0.0,0.0,0.0,0.0,0.0 +0,0.480300,0.0,0.0,0.0,0.0,0.0 +0,0.480400,0.0,0.0,0.0,0.0,0.0 +0,0.480500,0.0,0.0,0.0,0.0,0.0 +0,0.480600,0.0,0.0,0.0,0.0,0.0 +0,0.480700,0.0,0.0,0.0,0.0,0.0 +0,0.480800,0.0,0.0,0.0,0.0,0.0 +0,0.480900,0.0,0.0,0.0,0.0,0.0 +0,0.481000,0.0,0.0,0.0,0.0,0.0 +0,0.481100,0.0,0.0,0.0,0.0,0.0 +0,0.481200,0.0,0.0,0.0,0.0,0.0 +0,0.481300,0.0,0.0,0.0,0.0,0.0 +0,0.481400,0.0,0.0,0.0,0.0,0.0 +0,0.481500,0.0,0.0,0.0,0.0,0.0 +0,0.481600,0.0,0.0,0.0,0.0,0.0 +0,0.481700,0.0,0.0,0.0,0.0,0.0 +0,0.481800,0.0,0.0,0.0,0.0,0.0 +0,0.481900,0.0,0.0,0.0,0.0,0.0 +0,0.482000,0.0,0.0,0.0,0.0,0.0 +0,0.482100,0.0,0.0,0.0,0.0,0.0 +0,0.482200,0.0,0.0,0.0,0.0,0.0 +0,0.482300,0.0,0.0,0.0,0.0,0.0 +0,0.482400,0.0,0.0,0.0,0.0,0.0 +0,0.482500,0.0,0.0,0.0,0.0,0.0 +0,0.482600,0.0,0.0,0.0,0.0,0.0 +0,0.482700,0.0,0.0,0.0,0.0,0.0 +0,0.482800,0.0,0.0,0.0,0.0,0.0 +0,0.482900,0.0,0.0,0.0,0.0,0.0 +0,0.483000,0.0,0.0,0.0,0.0,0.0 +0,0.483100,0.0,0.0,0.0,0.0,0.0 +0,0.483200,0.0,0.0,0.0,0.0,0.0 +0,0.483300,0.0,0.0,0.0,0.0,0.0 +0,0.483400,0.0,0.0,0.0,0.0,0.0 +0,0.483500,0.0,0.0,0.0,0.0,0.0 +0,0.483600,0.0,0.0,0.0,0.0,0.0 +0,0.483700,0.0,0.0,0.0,0.0,0.0 +0,0.483800,0.0,0.0,0.0,0.0,0.0 +0,0.483900,0.0,0.0,0.0,0.0,0.0 +0,0.484000,0.0,0.0,0.0,0.0,0.0 +0,0.484100,0.0,0.0,0.0,0.0,0.0 +0,0.484200,0.0,0.0,0.0,0.0,0.0 +0,0.484300,0.0,0.0,0.0,0.0,0.0 +0,0.484400,0.0,0.0,0.0,0.0,0.0 +0,0.484500,0.0,0.0,0.0,0.0,0.0 +0,0.484600,0.0,0.0,0.0,0.0,0.0 +0,0.484700,0.0,0.0,0.0,0.0,0.0 +0,0.484800,0.0,0.0,0.0,0.0,0.0 +0,0.484900,0.0,0.0,0.0,0.0,0.0 +0,0.485000,0.0,0.0,0.0,0.0,0.0 +0,0.485100,0.0,0.0,0.0,0.0,0.0 +0,0.485200,0.0,0.0,0.0,0.0,0.0 +0,0.485300,0.0,0.0,0.0,0.0,0.0 +0,0.485400,0.0,0.0,0.0,0.0,0.0 +0,0.485500,0.0,0.0,0.0,0.0,0.0 +0,0.485600,0.0,0.0,0.0,0.0,0.0 +0,0.485700,0.0,0.0,0.0,0.0,0.0 +0,0.485800,0.0,0.0,0.0,0.0,0.0 +0,0.485900,0.0,0.0,0.0,0.0,0.0 +0,0.486000,0.0,0.0,0.0,0.0,0.0 +0,0.486100,0.0,0.0,0.0,0.0,0.0 +0,0.486200,0.0,0.0,0.0,0.0,0.0 +0,0.486300,0.0,0.0,0.0,0.0,0.0 +0,0.486400,0.0,0.0,0.0,0.0,0.0 +0,0.486500,0.0,0.0,0.0,0.0,0.0 +0,0.486600,0.0,0.0,0.0,0.0,0.0 +0,0.486700,0.0,0.0,0.0,0.0,0.0 +0,0.486800,0.0,0.0,0.0,0.0,0.0 +0,0.486900,0.0,0.0,0.0,0.0,0.0 +0,0.487000,0.0,0.0,0.0,0.0,0.0 +0,0.487100,0.0,0.0,0.0,0.0,0.0 +0,0.487200,0.0,0.0,0.0,0.0,0.0 +0,0.487300,0.0,0.0,0.0,0.0,0.0 +0,0.487400,0.0,0.0,0.0,0.0,0.0 +0,0.487500,0.0,0.0,0.0,0.0,0.0 +0,0.487600,0.0,0.0,0.0,0.0,0.0 +0,0.487700,0.0,0.0,0.0,0.0,0.0 +0,0.487800,0.0,0.0,0.0,0.0,0.0 +0,0.487900,0.0,0.0,0.0,0.0,0.0 +0,0.488000,0.0,0.0,0.0,0.0,0.0 +0,0.488100,0.0,0.0,0.0,0.0,0.0 +0,0.488200,0.0,0.0,0.0,0.0,0.0 +0,0.488300,0.0,0.0,0.0,0.0,0.0 +0,0.488400,0.0,0.0,0.0,0.0,0.0 +0,0.488500,0.0,0.0,0.0,0.0,0.0 +0,0.488600,0.0,0.0,0.0,0.0,0.0 +0,0.488700,0.0,0.0,0.0,0.0,0.0 +0,0.488800,0.0,0.0,0.0,0.0,0.0 +0,0.488900,0.0,0.0,0.0,0.0,0.0 +0,0.489000,0.0,0.0,0.0,0.0,0.0 +0,0.489100,0.0,0.0,0.0,0.0,0.0 +0,0.489200,0.0,0.0,0.0,0.0,0.0 +0,0.489300,0.0,0.0,0.0,0.0,0.0 +0,0.489400,0.0,0.0,0.0,0.0,0.0 +0,0.489500,0.0,0.0,0.0,0.0,0.0 +0,0.489600,0.0,0.0,0.0,0.0,0.0 +0,0.489700,0.0,0.0,0.0,0.0,0.0 +0,0.489800,0.0,0.0,0.0,0.0,0.0 +0,0.489900,0.0,0.0,0.0,0.0,0.0 +0,0.490000,0.0,0.0,0.0,0.0,0.0 +0,0.490100,0.0,0.0,0.0,0.0,0.0 +0,0.490200,0.0,0.0,0.0,0.0,0.0 +0,0.490300,0.0,0.0,0.0,0.0,0.0 +0,0.490400,0.0,0.0,0.0,0.0,0.0 +0,0.490500,0.0,0.0,0.0,0.0,0.0 +0,0.490600,0.0,0.0,0.0,0.0,0.0 +0,0.490700,0.0,0.0,0.0,0.0,0.0 +0,0.490800,0.0,0.0,0.0,0.0,0.0 +0,0.490900,0.0,0.0,0.0,0.0,0.0 +0,0.491000,0.0,0.0,0.0,0.0,0.0 +0,0.491100,0.0,0.0,0.0,0.0,0.0 +0,0.491200,0.0,0.0,0.0,0.0,0.0 +0,0.491300,0.0,0.0,0.0,0.0,0.0 +0,0.491400,0.0,0.0,0.0,0.0,0.0 +0,0.491500,0.0,0.0,0.0,0.0,0.0 +0,0.491600,0.0,0.0,0.0,0.0,0.0 +0,0.491700,0.0,0.0,0.0,0.0,0.0 +0,0.491800,0.0,0.0,0.0,0.0,0.0 +0,0.491900,0.0,0.0,0.0,0.0,0.0 +0,0.492000,0.0,0.0,0.0,0.0,0.0 +0,0.492100,0.0,0.0,0.0,0.0,0.0 +0,0.492200,0.0,0.0,0.0,0.0,0.0 +0,0.492300,0.0,0.0,0.0,0.0,0.0 +0,0.492400,0.0,0.0,0.0,0.0,0.0 +0,0.492500,0.0,0.0,0.0,0.0,0.0 +0,0.492600,0.0,0.0,0.0,0.0,0.0 +0,0.492700,0.0,0.0,0.0,0.0,0.0 +0,0.492800,0.0,0.0,0.0,0.0,0.0 +0,0.492900,0.0,0.0,0.0,0.0,0.0 +0,0.493000,0.0,0.0,0.0,0.0,0.0 +0,0.493100,0.0,0.0,0.0,0.0,0.0 +0,0.493200,0.0,0.0,0.0,0.0,0.0 +0,0.493300,0.0,0.0,0.0,0.0,0.0 +0,0.493400,0.0,0.0,0.0,0.0,0.0 +0,0.493500,0.0,0.0,0.0,0.0,0.0 +0,0.493600,0.0,0.0,0.0,0.0,0.0 +0,0.493700,0.0,0.0,0.0,0.0,0.0 +0,0.493800,0.0,0.0,0.0,0.0,0.0 +0,0.493900,0.0,0.0,0.0,0.0,0.0 +0,0.494000,0.0,0.0,0.0,0.0,0.0 +0,0.494100,0.0,0.0,0.0,0.0,0.0 +0,0.494200,0.0,0.0,0.0,0.0,0.0 +0,0.494300,0.0,0.0,0.0,0.0,0.0 +0,0.494400,0.0,0.0,0.0,0.0,0.0 +0,0.494500,0.0,0.0,0.0,0.0,0.0 +0,0.494600,0.0,0.0,0.0,0.0,0.0 +0,0.494700,0.0,0.0,0.0,0.0,0.0 +0,0.494800,0.0,0.0,0.0,0.0,0.0 +0,0.494900,0.0,0.0,0.0,0.0,0.0 +0,0.495000,0.0,0.0,0.0,0.0,0.0 +0,0.495100,0.0,0.0,0.0,0.0,0.0 +0,0.495200,0.0,0.0,0.0,0.0,0.0 +0,0.495300,0.0,0.0,0.0,0.0,0.0 +0,0.495400,0.0,0.0,0.0,0.0,0.0 +0,0.495500,0.0,0.0,0.0,0.0,0.0 +0,0.495600,0.0,0.0,0.0,0.0,0.0 +0,0.495700,0.0,0.0,0.0,0.0,0.0 +0,0.495800,0.0,0.0,0.0,0.0,0.0 +0,0.495900,0.0,0.0,0.0,0.0,0.0 +0,0.496000,0.0,0.0,0.0,0.0,0.0 +0,0.496100,0.0,0.0,0.0,0.0,0.0 +0,0.496200,0.0,0.0,0.0,0.0,0.0 +0,0.496300,0.0,0.0,0.0,0.0,0.0 +0,0.496400,0.0,0.0,0.0,0.0,0.0 +0,0.496500,0.0,0.0,0.0,0.0,0.0 +0,0.496600,0.0,0.0,0.0,0.0,0.0 +0,0.496700,0.0,0.0,0.0,0.0,0.0 +0,0.496800,0.0,0.0,0.0,0.0,0.0 +0,0.496900,0.0,0.0,0.0,0.0,0.0 +0,0.497000,0.0,0.0,0.0,0.0,0.0 +0,0.497100,0.0,0.0,0.0,0.0,0.0 +0,0.497200,0.0,0.0,0.0,0.0,0.0 +0,0.497300,0.0,0.0,0.0,0.0,0.0 +0,0.497400,0.0,0.0,0.0,0.0,0.0 +0,0.497500,0.0,0.0,0.0,0.0,0.0 +0,0.497600,0.0,0.0,0.0,0.0,0.0 +0,0.497700,0.0,0.0,0.0,0.0,0.0 +0,0.497800,0.0,0.0,0.0,0.0,0.0 +0,0.497900,0.0,0.0,0.0,0.0,0.0 +0,0.498000,0.0,0.0,0.0,0.0,0.0 +0,0.498100,0.0,0.0,0.0,0.0,0.0 +0,0.498200,0.0,0.0,0.0,0.0,0.0 +0,0.498300,0.0,0.0,0.0,0.0,0.0 +0,0.498400,0.0,0.0,0.0,0.0,0.0 +0,0.498500,0.0,0.0,0.0,0.0,0.0 +0,0.498600,0.0,0.0,0.0,0.0,0.0 +0,0.498700,0.0,0.0,0.0,0.0,0.0 +0,0.498800,0.0,0.0,0.0,0.0,0.0 +0,0.498900,0.0,0.0,0.0,0.0,0.0 +0,0.499000,0.0,0.0,0.0,0.0,0.0 +0,0.499100,0.0,0.0,0.0,0.0,0.0 +0,0.499200,0.0,0.0,0.0,0.0,0.0 +0,0.499300,0.0,0.0,0.0,0.0,0.0 +0,0.499400,0.0,0.0,0.0,0.0,0.0 +0,0.499500,0.0,0.0,0.0,0.0,0.0 +0,0.499600,0.0,0.0,0.0,0.0,0.0 +0,0.499700,0.0,0.0,0.0,0.0,0.0 +0,0.499800,0.0,0.0,0.0,0.0,0.0 +0,0.499900,0.0,0.0,0.0,0.0,0.0 +0,0.500000,0.0,0.0,0.0,0.0,0.0 +0,0.500100,0.0,0.0,0.0,0.0,0.0 +1,52.130222,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.500200,0.0,0.0,0.0,0.0,0.0 +0,0.500300,0.0,0.0,0.0,0.0,0.0 +0,0.500400,0.0,0.0,0.0,0.0,0.0 +0,0.500500,0.0,0.0,0.0,0.0,0.0 +0,0.500600,0.0,0.0,0.0,0.0,0.0 +0,0.500700,0.0,0.0,0.0,0.0,0.0 +0,0.500800,0.0,0.0,0.0,0.0,0.0 +0,0.500900,0.0,0.0,0.0,0.0,0.0 +0,0.501000,0.0,0.0,0.0,0.0,0.0 +0,0.501100,0.0,0.0,0.0,0.0,0.0 +0,0.501200,0.0,0.0,0.0,0.0,0.0 +0,0.501300,0.0,0.0,0.0,0.0,0.0 +0,0.501400,0.0,0.0,0.0,0.0,0.0 +0,0.501500,0.0,0.0,0.0,0.0,0.0 +0,0.501600,0.0,0.0,0.0,0.0,0.0 +0,0.501700,0.0,0.0,0.0,0.0,0.0 +0,0.501800,0.0,0.0,0.0,0.0,0.0 +0,0.501900,0.0,0.0,0.0,0.0,0.0 +0,0.502000,0.0,0.0,0.0,0.0,0.0 +0,0.502100,0.0,0.0,0.0,0.0,0.0 +0,0.502200,0.0,0.0,0.0,0.0,0.0 +0,0.502300,0.0,0.0,0.0,0.0,0.0 +0,0.502400,0.0,0.0,0.0,0.0,0.0 +0,0.502500,0.0,0.0,0.0,0.0,0.0 +0,0.502600,0.0,0.0,0.0,0.0,0.0 +0,0.502700,0.0,0.0,0.0,0.0,0.0 +0,0.502800,0.0,0.0,0.0,0.0,0.0 +0,0.502900,0.0,0.0,0.0,0.0,0.0 +0,0.503000,0.0,0.0,0.0,0.0,0.0 +0,0.503100,0.0,0.0,0.0,0.0,0.0 +0,0.503200,0.0,0.0,0.0,0.0,0.0 +0,0.503300,0.0,0.0,0.0,0.0,0.0 +0,0.503400,0.0,0.0,0.0,0.0,0.0 +0,0.503500,0.0,0.0,0.0,0.0,0.0 +0,0.503600,0.0,0.0,0.0,0.0,0.0 +0,0.503700,0.0,0.0,0.0,0.0,0.0 +0,0.503800,0.0,0.0,0.0,0.0,0.0 +0,0.503900,0.0,0.0,0.0,0.0,0.0 +0,0.504000,0.0,0.0,0.0,0.0,0.0 +0,0.504100,0.0,0.0,0.0,0.0,0.0 +0,0.504200,0.0,0.0,0.0,0.0,0.0 +0,0.504300,0.0,0.0,0.0,0.0,0.0 +0,0.504400,0.0,0.0,0.0,0.0,0.0 +0,0.504500,0.0,0.0,0.0,0.0,0.0 +0,0.504600,0.0,0.0,0.0,0.0,0.0 +0,0.504700,0.0,0.0,0.0,0.0,0.0 +0,0.504800,0.0,0.0,0.0,0.0,0.0 +0,0.504900,0.0,0.0,0.0,0.0,0.0 +0,0.505000,0.0,0.0,0.0,0.0,0.0 +0,0.505100,0.0,0.0,0.0,0.0,0.0 +0,0.505200,0.0,0.0,0.0,0.0,0.0 +0,0.505300,0.0,0.0,0.0,0.0,0.0 +0,0.505400,0.0,0.0,0.0,0.0,0.0 +0,0.505500,0.0,0.0,0.0,0.0,0.0 +0,0.505600,0.0,0.0,0.0,0.0,0.0 +0,0.505700,0.0,0.0,0.0,0.0,0.0 +0,0.505800,0.0,0.0,0.0,0.0,0.0 +0,0.505900,0.0,0.0,0.0,0.0,0.0 +0,0.506000,0.0,0.0,0.0,0.0,0.0 +0,0.506100,0.0,0.0,0.0,0.0,0.0 +0,0.506200,0.0,0.0,0.0,0.0,0.0 +0,0.506300,0.0,0.0,0.0,0.0,0.0 +0,0.506400,0.0,0.0,0.0,0.0,0.0 +0,0.506500,0.0,0.0,0.0,0.0,0.0 +0,0.506600,0.0,0.0,0.0,0.0,0.0 +0,0.506700,0.0,0.0,0.0,0.0,0.0 +0,0.506800,0.0,0.0,0.0,0.0,0.0 +0,0.506900,0.0,0.0,0.0,0.0,0.0 +0,0.507000,0.0,0.0,0.0,0.0,0.0 +0,0.507100,0.0,0.0,0.0,0.0,0.0 +0,0.507200,0.0,0.0,0.0,0.0,0.0 +0,0.507300,0.0,0.0,0.0,0.0,0.0 +0,0.507400,0.0,0.0,0.0,0.0,0.0 +0,0.507500,0.0,0.0,0.0,0.0,0.0 +0,0.507600,0.0,0.0,0.0,0.0,0.0 +0,0.507700,0.0,0.0,0.0,0.0,0.0 +0,0.507800,0.0,0.0,0.0,0.0,0.0 +0,0.507900,0.0,0.0,0.0,0.0,0.0 +0,0.508000,0.0,0.0,0.0,0.0,0.0 +0,0.508100,0.0,0.0,0.0,0.0,0.0 +0,0.508200,0.0,0.0,0.0,0.0,0.0 +0,0.508300,0.0,0.0,0.0,0.0,0.0 +0,0.508400,0.0,0.0,0.0,0.0,0.0 +0,0.508500,0.0,0.0,0.0,0.0,0.0 +0,0.508600,0.0,0.0,0.0,0.0,0.0 +0,0.508700,0.0,0.0,0.0,0.0,0.0 +0,0.508800,0.0,0.0,0.0,0.0,0.0 +0,0.508900,0.0,0.0,0.0,0.0,0.0 +0,0.509000,0.0,0.0,0.0,0.0,0.0 +0,0.509100,0.0,0.0,0.0,0.0,0.0 +0,0.509200,0.0,0.0,0.0,0.0,0.0 +0,0.509300,0.0,0.0,0.0,0.0,0.0 +0,0.509400,0.0,0.0,0.0,0.0,0.0 +0,0.509500,0.0,0.0,0.0,0.0,0.0 +0,0.509600,0.0,0.0,0.0,0.0,0.0 +0,0.509700,0.0,0.0,0.0,0.0,0.0 +0,0.509800,0.0,0.0,0.0,0.0,0.0 +0,0.509900,0.0,0.0,0.0,0.0,0.0 +0,0.510000,0.0,0.0,0.0,0.0,0.0 +0,0.510100,0.0,0.0,0.0,0.0,0.0 +0,0.510200,0.0,0.0,0.0,0.0,0.0 +0,0.510300,0.0,0.0,0.0,0.0,0.0 +0,0.510400,0.0,0.0,0.0,0.0,0.0 +0,0.510500,0.0,0.0,0.0,0.0,0.0 +0,0.510600,0.0,0.0,0.0,0.0,0.0 +0,0.510700,0.0,0.0,0.0,0.0,0.0 +0,0.510800,0.0,0.0,0.0,0.0,0.0 +0,0.510900,0.0,0.0,0.0,0.0,0.0 +0,0.511000,0.0,0.0,0.0,0.0,0.0 +0,0.511100,0.0,0.0,0.0,0.0,0.0 +0,0.511200,0.0,0.0,0.0,0.0,0.0 +0,0.511300,0.0,0.0,0.0,0.0,0.0 +0,0.511400,0.0,0.0,0.0,0.0,0.0 +0,0.511500,0.0,0.0,0.0,0.0,0.0 +0,0.511600,0.0,0.0,0.0,0.0,0.0 +0,0.511700,0.0,0.0,0.0,0.0,0.0 +0,0.511800,0.0,0.0,0.0,0.0,0.0 +0,0.511900,0.0,0.0,0.0,0.0,0.0 +0,0.512000,0.0,0.0,0.0,0.0,0.0 +0,0.512100,0.0,0.0,0.0,0.0,0.0 +0,0.512200,0.0,0.0,0.0,0.0,0.0 +0,0.512300,0.0,0.0,0.0,0.0,0.0 +0,0.512400,0.0,0.0,0.0,0.0,0.0 +0,0.512500,0.0,0.0,0.0,0.0,0.0 +0,0.512600,0.0,0.0,0.0,0.0,0.0 +0,0.512700,0.0,0.0,0.0,0.0,0.0 +0,0.512800,0.0,0.0,0.0,0.0,0.0 +0,0.512900,0.0,0.0,0.0,0.0,0.0 +0,0.513000,0.0,0.0,0.0,0.0,0.0 +0,0.513100,0.0,0.0,0.0,0.0,0.0 +0,0.513200,0.0,0.0,0.0,0.0,0.0 +0,0.513300,0.0,0.0,0.0,0.0,0.0 +0,0.513400,0.0,0.0,0.0,0.0,0.0 +0,0.513500,0.0,0.0,0.0,0.0,0.0 +0,0.513600,0.0,0.0,0.0,0.0,0.0 +0,0.513700,0.0,0.0,0.0,0.0,0.0 +0,0.513800,0.0,0.0,0.0,0.0,0.0 +0,0.513900,0.0,0.0,0.0,0.0,0.0 +0,0.514000,0.0,0.0,0.0,0.0,0.0 +0,0.514100,0.0,0.0,0.0,0.0,0.0 +0,0.514200,0.0,0.0,0.0,0.0,0.0 +0,0.514300,0.0,0.0,0.0,0.0,0.0 +0,0.514400,0.0,0.0,0.0,0.0,0.0 +0,0.514500,0.0,0.0,0.0,0.0,0.0 +0,0.514600,0.0,0.0,0.0,0.0,0.0 +0,0.514700,0.0,0.0,0.0,0.0,0.0 +0,0.514800,0.0,0.0,0.0,0.0,0.0 +0,0.514900,0.0,0.0,0.0,0.0,0.0 +0,0.515000,0.0,0.0,0.0,0.0,0.0 +0,0.515100,0.0,0.0,0.0,0.0,0.0 +0,0.515200,0.0,0.0,0.0,0.0,0.0 +0,0.515300,0.0,0.0,0.0,0.0,0.0 +0,0.515400,0.0,0.0,0.0,0.0,0.0 +0,0.515500,0.0,0.0,0.0,0.0,0.0 +0,0.515600,0.0,0.0,0.0,0.0,0.0 +0,0.515700,0.0,0.0,0.0,0.0,0.0 +0,0.515800,0.0,0.0,0.0,0.0,0.0 +0,0.515900,0.0,0.0,0.0,0.0,0.0 +0,0.516000,0.0,0.0,0.0,0.0,0.0 +0,0.516100,0.0,0.0,0.0,0.0,0.0 +0,0.516200,0.0,0.0,0.0,0.0,0.0 +0,0.516300,0.0,0.0,0.0,0.0,0.0 +0,0.516400,0.0,0.0,0.0,0.0,0.0 +0,0.516500,0.0,0.0,0.0,0.0,0.0 +0,0.516600,0.0,0.0,0.0,0.0,0.0 +0,0.516700,0.0,0.0,0.0,0.0,0.0 +0,0.516800,0.0,0.0,0.0,0.0,0.0 +0,0.516900,0.0,0.0,0.0,0.0,0.0 +0,0.517000,0.0,0.0,0.0,0.0,0.0 +0,0.517100,0.0,0.0,0.0,0.0,0.0 +0,0.517200,0.0,0.0,0.0,0.0,0.0 +0,0.517300,0.0,0.0,0.0,0.0,0.0 +0,0.517400,0.0,0.0,0.0,0.0,0.0 +0,0.517500,0.0,0.0,0.0,0.0,0.0 +0,0.517600,0.0,0.0,0.0,0.0,0.0 +0,0.517700,0.0,0.0,0.0,0.0,0.0 +0,0.517800,0.0,0.0,0.0,0.0,0.0 +0,0.517900,0.0,0.0,0.0,0.0,0.0 +0,0.518000,0.0,0.0,0.0,0.0,0.0 +0,0.518100,0.0,0.0,0.0,0.0,0.0 +0,0.518200,0.0,0.0,0.0,0.0,0.0 +0,0.518300,0.0,0.0,0.0,0.0,0.0 +0,0.518400,0.0,0.0,0.0,0.0,0.0 +0,0.518500,0.0,0.0,0.0,0.0,0.0 +0,0.518600,0.0,0.0,0.0,0.0,0.0 +0,0.518700,0.0,0.0,0.0,0.0,0.0 +0,0.518800,0.0,0.0,0.0,0.0,0.0 +0,0.518900,0.0,0.0,0.0,0.0,0.0 +0,0.519000,0.0,0.0,0.0,0.0,0.0 +0,0.519100,0.0,0.0,0.0,0.0,0.0 +0,0.519200,0.0,0.0,0.0,0.0,0.0 +0,0.519300,0.0,0.0,0.0,0.0,0.0 +0,0.519400,0.0,0.0,0.0,0.0,0.0 +0,0.519500,0.0,0.0,0.0,0.0,0.0 +0,0.519600,0.0,0.0,0.0,0.0,0.0 +0,0.519700,0.0,0.0,0.0,0.0,0.0 +0,0.519800,0.0,0.0,0.0,0.0,0.0 +0,0.519900,0.0,0.0,0.0,0.0,0.0 +0,0.520000,0.0,0.0,0.0,0.0,0.0 +0,0.520100,0.0,0.0,0.0,0.0,0.0 +1,58.637381,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.520200,0.0,0.0,0.0,0.0,0.0 +0,0.520300,0.0,0.0,0.0,0.0,0.0 +0,0.520400,0.0,0.0,0.0,0.0,0.0 +0,0.520500,0.0,0.0,0.0,0.0,0.0 +0,0.520600,0.0,0.0,0.0,0.0,0.0 +0,0.520700,0.0,0.0,0.0,0.0,0.0 +0,0.520800,0.0,0.0,0.0,0.0,0.0 +0,0.520900,0.0,0.0,0.0,0.0,0.0 +0,0.521000,0.0,0.0,0.0,0.0,0.0 +0,0.521100,0.0,0.0,0.0,0.0,0.0 +0,0.521200,0.0,0.0,0.0,0.0,0.0 +0,0.521300,0.0,0.0,0.0,0.0,0.0 +0,0.521400,0.0,0.0,0.0,0.0,0.0 +0,0.521500,0.0,0.0,0.0,0.0,0.0 +0,0.521600,0.0,0.0,0.0,0.0,0.0 +0,0.521700,0.0,0.0,0.0,0.0,0.0 +0,0.521800,0.0,0.0,0.0,0.0,0.0 +0,0.521900,0.0,0.0,0.0,0.0,0.0 +0,0.522000,0.0,0.0,0.0,0.0,0.0 +0,0.522100,0.0,0.0,0.0,0.0,0.0 +0,0.522200,0.0,0.0,0.0,0.0,0.0 +0,0.522300,0.0,0.0,0.0,0.0,0.0 +0,0.522400,0.0,0.0,0.0,0.0,0.0 +0,0.522500,0.0,0.0,0.0,0.0,0.0 +0,0.522600,0.0,0.0,0.0,0.0,0.0 +0,0.522700,0.0,0.0,0.0,0.0,0.0 +0,0.522800,0.0,0.0,0.0,0.0,0.0 +0,0.522900,0.0,0.0,0.0,0.0,0.0 +0,0.523000,0.0,0.0,0.0,0.0,0.0 +0,0.523100,0.0,0.0,0.0,0.0,0.0 +0,0.523200,0.0,0.0,0.0,0.0,0.0 +0,0.523300,0.0,0.0,0.0,0.0,0.0 +0,0.523400,0.0,0.0,0.0,0.0,0.0 +0,0.523500,0.0,0.0,0.0,0.0,0.0 +0,0.523600,0.0,0.0,0.0,0.0,0.0 +0,0.523700,0.0,0.0,0.0,0.0,0.0 +0,0.523800,0.0,0.0,0.0,0.0,0.0 +0,0.523900,0.0,0.0,0.0,0.0,0.0 +0,0.524000,0.0,0.0,0.0,0.0,0.0 +0,0.524100,0.0,0.0,0.0,0.0,0.0 +0,0.524200,0.0,0.0,0.0,0.0,0.0 +0,0.524300,0.0,0.0,0.0,0.0,0.0 +0,0.524400,0.0,0.0,0.0,0.0,0.0 +0,0.524500,0.0,0.0,0.0,0.0,0.0 +0,0.524600,0.0,0.0,0.0,0.0,0.0 +0,0.524700,0.0,0.0,0.0,0.0,0.0 +0,0.524800,0.0,0.0,0.0,0.0,0.0 +0,0.524900,0.0,0.0,0.0,0.0,0.0 +0,0.525000,0.0,0.0,0.0,0.0,0.0 +0,0.525100,0.0,0.0,0.0,0.0,0.0 +0,0.525200,0.0,0.0,0.0,0.0,0.0 +0,0.525300,0.0,0.0,0.0,0.0,0.0 +0,0.525400,0.0,0.0,0.0,0.0,0.0 +0,0.525500,0.0,0.0,0.0,0.0,0.0 +0,0.525600,0.0,0.0,0.0,0.0,0.0 +0,0.525700,0.0,0.0,0.0,0.0,0.0 +0,0.525800,0.0,0.0,0.0,0.0,0.0 +0,0.525900,0.0,0.0,0.0,0.0,0.0 +0,0.526000,0.0,0.0,0.0,0.0,0.0 +0,0.526100,0.0,0.0,0.0,0.0,0.0 +0,0.526200,0.0,0.0,0.0,0.0,0.0 +0,0.526300,0.0,0.0,0.0,0.0,0.0 +0,0.526400,0.0,0.0,0.0,0.0,0.0 +0,0.526500,0.0,0.0,0.0,0.0,0.0 +0,0.526600,0.0,0.0,0.0,0.0,0.0 +0,0.526700,0.0,0.0,0.0,0.0,0.0 +0,0.526800,0.0,0.0,0.0,0.0,0.0 +0,0.526900,0.0,0.0,0.0,0.0,0.0 +0,0.527000,0.0,0.0,0.0,0.0,0.0 +0,0.527100,0.0,0.0,0.0,0.0,0.0 +0,0.527200,0.0,0.0,0.0,0.0,0.0 +0,0.527300,0.0,0.0,0.0,0.0,0.0 +0,0.527400,0.0,0.0,0.0,0.0,0.0 +0,0.527500,0.0,0.0,0.0,0.0,0.0 +0,0.527600,0.0,0.0,0.0,0.0,0.0 +0,0.527700,0.0,0.0,0.0,0.0,0.0 +0,0.527800,0.0,0.0,0.0,0.0,0.0 +0,0.527900,0.0,0.0,0.0,0.0,0.0 +0,0.528000,0.0,0.0,0.0,0.0,0.0 +0,0.528100,0.0,0.0,0.0,0.0,0.0 +0,0.528200,0.0,0.0,0.0,0.0,0.0 +0,0.528300,0.0,0.0,0.0,0.0,0.0 +0,0.528400,0.0,0.0,0.0,0.0,0.0 +0,0.528500,0.0,0.0,0.0,0.0,0.0 +0,0.528600,0.0,0.0,0.0,0.0,0.0 +0,0.528700,0.0,0.0,0.0,0.0,0.0 +0,0.528800,0.0,0.0,0.0,0.0,0.0 +0,0.528900,0.0,0.0,0.0,0.0,0.0 +0,0.529000,0.0,0.0,0.0,0.0,0.0 +0,0.529100,0.0,0.0,0.0,0.0,0.0 +0,0.529200,0.0,0.0,0.0,0.0,0.0 +0,0.529300,0.0,0.0,0.0,0.0,0.0 +0,0.529400,0.0,0.0,0.0,0.0,0.0 +0,0.529500,0.0,0.0,0.0,0.0,0.0 +0,0.529600,0.0,0.0,0.0,0.0,0.0 +0,0.529700,0.0,0.0,0.0,0.0,0.0 +0,0.529800,0.0,0.0,0.0,0.0,0.0 +0,0.529900,0.0,0.0,0.0,0.0,0.0 +0,0.530000,0.0,0.0,0.0,0.0,0.0 +0,0.530100,0.0,0.0,0.0,0.0,0.0 +0,0.530200,0.0,0.0,0.0,0.0,0.0 +0,0.530300,0.0,0.0,0.0,0.0,0.0 +0,0.530400,0.0,0.0,0.0,0.0,0.0 +0,0.530500,0.0,0.0,0.0,0.0,0.0 +0,0.530600,0.0,0.0,0.0,0.0,0.0 +0,0.530700,0.0,0.0,0.0,0.0,0.0 +0,0.530800,0.0,0.0,0.0,0.0,0.0 +0,0.530900,0.0,0.0,0.0,0.0,0.0 +0,0.531000,0.0,0.0,0.0,0.0,0.0 +0,0.531100,0.0,0.0,0.0,0.0,0.0 +0,0.531200,0.0,0.0,0.0,0.0,0.0 +0,0.531300,0.0,0.0,0.0,0.0,0.0 +0,0.531400,0.0,0.0,0.0,0.0,0.0 +0,0.531500,0.0,0.0,0.0,0.0,0.0 +0,0.531600,0.0,0.0,0.0,0.0,0.0 +0,0.531700,0.0,0.0,0.0,0.0,0.0 +0,0.531800,0.0,0.0,0.0,0.0,0.0 +0,0.531900,0.0,0.0,0.0,0.0,0.0 +0,0.532000,0.0,0.0,0.0,0.0,0.0 +0,0.532100,0.0,0.0,0.0,0.0,0.0 +0,0.532200,0.0,0.0,0.0,0.0,0.0 +0,0.532300,0.0,0.0,0.0,0.0,0.0 +0,0.532400,0.0,0.0,0.0,0.0,0.0 +0,0.532500,0.0,0.0,0.0,0.0,0.0 +0,0.532600,0.0,0.0,0.0,0.0,0.0 +0,0.532700,0.0,0.0,0.0,0.0,0.0 +0,0.532800,0.0,0.0,0.0,0.0,0.0 +0,0.532900,0.0,0.0,0.0,0.0,0.0 +0,0.533000,0.0,0.0,0.0,0.0,0.0 +0,0.533100,0.0,0.0,0.0,0.0,0.0 +0,0.533200,0.0,0.0,0.0,0.0,0.0 +0,0.533300,0.0,0.0,0.0,0.0,0.0 +0,0.533400,0.0,0.0,0.0,0.0,0.0 +0,0.533500,0.0,0.0,0.0,0.0,0.0 +0,0.533600,0.0,0.0,0.0,0.0,0.0 +0,0.533700,0.0,0.0,0.0,0.0,0.0 +0,0.533800,0.0,0.0,0.0,0.0,0.0 +0,0.533900,0.0,0.0,0.0,0.0,0.0 +0,0.534000,0.0,0.0,0.0,0.0,0.0 +0,0.534100,0.0,0.0,0.0,0.0,0.0 +0,0.534200,0.0,0.0,0.0,0.0,0.0 +0,0.534300,0.0,0.0,0.0,0.0,0.0 +0,0.534400,0.0,0.0,0.0,0.0,0.0 +0,0.534500,0.0,0.0,0.0,0.0,0.0 +0,0.534600,0.0,0.0,0.0,0.0,0.0 +0,0.534700,0.0,0.0,0.0,0.0,0.0 +0,0.534800,0.0,0.0,0.0,0.0,0.0 +0,0.534900,0.0,0.0,0.0,0.0,0.0 +0,0.535000,0.0,0.0,0.0,0.0,0.0 +0,0.535100,0.0,0.0,0.0,0.0,0.0 +0,0.535200,0.0,0.0,0.0,0.0,0.0 +0,0.535300,0.0,0.0,0.0,0.0,0.0 +0,0.535400,0.0,0.0,0.0,0.0,0.0 +0,0.535500,0.0,0.0,0.0,0.0,0.0 +0,0.535600,0.0,0.0,0.0,0.0,0.0 +0,0.535700,0.0,0.0,0.0,0.0,0.0 +0,0.535800,0.0,0.0,0.0,0.0,0.0 +0,0.535900,0.0,0.0,0.0,0.0,0.0 +0,0.536000,0.0,0.0,0.0,0.0,0.0 +0,0.536100,0.0,0.0,0.0,0.0,0.0 +0,0.536200,0.0,0.0,0.0,0.0,0.0 +0,0.536300,0.0,0.0,0.0,0.0,0.0 +0,0.536400,0.0,0.0,0.0,0.0,0.0 +0,0.536500,0.0,0.0,0.0,0.0,0.0 +0,0.536600,0.0,0.0,0.0,0.0,0.0 +0,0.536700,0.0,0.0,0.0,0.0,0.0 +0,0.536800,0.0,0.0,0.0,0.0,0.0 +0,0.536900,0.0,0.0,0.0,0.0,0.0 +0,0.537000,0.0,0.0,0.0,0.0,0.0 +0,0.537100,0.0,0.0,0.0,0.0,0.0 +0,0.537200,0.0,0.0,0.0,0.0,0.0 +0,0.537300,0.0,0.0,0.0,0.0,0.0 +0,0.537400,0.0,0.0,0.0,0.0,0.0 +0,0.537500,0.0,0.0,0.0,0.0,0.0 +0,0.537600,0.0,0.0,0.0,0.0,0.0 +0,0.537700,0.0,0.0,0.0,0.0,0.0 +0,0.537800,0.0,0.0,0.0,0.0,0.0 +0,0.537900,0.0,0.0,0.0,0.0,0.0 +0,0.538000,0.0,0.0,0.0,0.0,0.0 +0,0.538100,0.0,0.0,0.0,0.0,0.0 +0,0.538200,0.0,0.0,0.0,0.0,0.0 +0,0.538300,0.0,0.0,0.0,0.0,0.0 +0,0.538400,0.0,0.0,0.0,0.0,0.0 +0,0.538500,0.0,0.0,0.0,0.0,0.0 +0,0.538600,0.0,0.0,0.0,0.0,0.0 +0,0.538700,0.0,0.0,0.0,0.0,0.0 +0,0.538800,0.0,0.0,0.0,0.0,0.0 +0,0.538900,0.0,0.0,0.0,0.0,0.0 +0,0.539000,0.0,0.0,0.0,0.0,0.0 +0,0.539100,0.0,0.0,0.0,0.0,0.0 +0,0.539200,0.0,0.0,0.0,0.0,0.0 +0,0.539300,0.0,0.0,0.0,0.0,0.0 +0,0.539400,0.0,0.0,0.0,0.0,0.0 +0,0.539500,0.0,0.0,0.0,0.0,0.0 +0,0.539600,0.0,0.0,0.0,0.0,0.0 +0,0.539700,0.0,0.0,0.0,0.0,0.0 +0,0.539800,0.0,0.0,0.0,0.0,0.0 +0,0.539900,0.0,0.0,0.0,0.0,0.0 +0,0.540000,0.0,0.0,0.0,0.0,0.0 +0,0.540100,0.0,0.0,0.0,0.0,0.0 +1,65.664690,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.540200,0.0,0.0,0.0,0.0,0.0 +0,0.540300,0.0,0.0,0.0,0.0,0.0 +0,0.540400,0.0,0.0,0.0,0.0,0.0 +0,0.540500,0.0,0.0,0.0,0.0,0.0 +0,0.540600,0.0,0.0,0.0,0.0,0.0 +0,0.540700,0.0,0.0,0.0,0.0,0.0 +0,0.540800,0.0,0.0,0.0,0.0,0.0 +0,0.540900,0.0,0.0,0.0,0.0,0.0 +0,0.541000,0.0,0.0,0.0,0.0,0.0 +0,0.541100,0.0,0.0,0.0,0.0,0.0 +0,0.541200,0.0,0.0,0.0,0.0,0.0 +0,0.541300,0.0,0.0,0.0,0.0,0.0 +0,0.541400,0.0,0.0,0.0,0.0,0.0 +0,0.541500,0.0,0.0,0.0,0.0,0.0 +0,0.541600,0.0,0.0,0.0,0.0,0.0 +0,0.541700,0.0,0.0,0.0,0.0,0.0 +0,0.541800,0.0,0.0,0.0,0.0,0.0 +0,0.541900,0.0,0.0,0.0,0.0,0.0 +0,0.542000,0.0,0.0,0.0,0.0,0.0 +0,0.542100,0.0,0.0,0.0,0.0,0.0 +0,0.542200,0.0,0.0,0.0,0.0,0.0 +0,0.542300,0.0,0.0,0.0,0.0,0.0 +0,0.542400,0.0,0.0,0.0,0.0,0.0 +0,0.542500,0.0,0.0,0.0,0.0,0.0 +0,0.542600,0.0,0.0,0.0,0.0,0.0 +0,0.542700,0.0,0.0,0.0,0.0,0.0 +0,0.542800,0.0,0.0,0.0,0.0,0.0 +0,0.542900,0.0,0.0,0.0,0.0,0.0 +0,0.543000,0.0,0.0,0.0,0.0,0.0 +0,0.543100,0.0,0.0,0.0,0.0,0.0 +0,0.543200,0.0,0.0,0.0,0.0,0.0 +0,0.543300,0.0,0.0,0.0,0.0,0.0 +0,0.543400,0.0,0.0,0.0,0.0,0.0 +0,0.543500,0.0,0.0,0.0,0.0,0.0 +0,0.543600,0.0,0.0,0.0,0.0,0.0 +0,0.543700,0.0,0.0,0.0,0.0,0.0 +0,0.543800,0.0,0.0,0.0,0.0,0.0 +0,0.543900,0.0,0.0,0.0,0.0,0.0 +0,0.544000,0.0,0.0,0.0,0.0,0.0 +0,0.544100,0.0,0.0,0.0,0.0,0.0 +0,0.544200,0.0,0.0,0.0,0.0,0.0 +0,0.544300,0.0,0.0,0.0,0.0,0.0 +0,0.544400,0.0,0.0,0.0,0.0,0.0 +0,0.544500,0.0,0.0,0.0,0.0,0.0 +0,0.544600,0.0,0.0,0.0,0.0,0.0 +0,0.544700,0.0,0.0,0.0,0.0,0.0 +0,0.544800,0.0,0.0,0.0,0.0,0.0 +0,0.544900,0.0,0.0,0.0,0.0,0.0 +0,0.545000,0.0,0.0,0.0,0.0,0.0 +0,0.545100,0.0,0.0,0.0,0.0,0.0 +0,0.545200,0.0,0.0,0.0,0.0,0.0 +0,0.545300,0.0,0.0,0.0,0.0,0.0 +0,0.545400,0.0,0.0,0.0,0.0,0.0 +0,0.545500,0.0,0.0,0.0,0.0,0.0 +0,0.545600,0.0,0.0,0.0,0.0,0.0 +0,0.545700,0.0,0.0,0.0,0.0,0.0 +0,0.545800,0.0,0.0,0.0,0.0,0.0 +0,0.545900,0.0,0.0,0.0,0.0,0.0 +0,0.546000,0.0,0.0,0.0,0.0,0.0 +0,0.546100,0.0,0.0,0.0,0.0,0.0 +0,0.546200,0.0,0.0,0.0,0.0,0.0 +0,0.546300,0.0,0.0,0.0,0.0,0.0 +0,0.546400,0.0,0.0,0.0,0.0,0.0 +0,0.546500,0.0,0.0,0.0,0.0,0.0 +0,0.546600,0.0,0.0,0.0,0.0,0.0 +0,0.546700,0.0,0.0,0.0,0.0,0.0 +0,0.546800,0.0,0.0,0.0,0.0,0.0 +0,0.546900,0.0,0.0,0.0,0.0,0.0 +0,0.547000,0.0,0.0,0.0,0.0,0.0 +0,0.547100,0.0,0.0,0.0,0.0,0.0 +0,0.547200,0.0,0.0,0.0,0.0,0.0 +0,0.547300,0.0,0.0,0.0,0.0,0.0 +0,0.547400,0.0,0.0,0.0,0.0,0.0 +0,0.547500,0.0,0.0,0.0,0.0,0.0 +0,0.547600,0.0,0.0,0.0,0.0,0.0 +0,0.547700,0.0,0.0,0.0,0.0,0.0 +0,0.547800,0.0,0.0,0.0,0.0,0.0 +0,0.547900,0.0,0.0,0.0,0.0,0.0 +0,0.548000,0.0,0.0,0.0,0.0,0.0 +0,0.548100,0.0,0.0,0.0,0.0,0.0 +0,0.548200,0.0,0.0,0.0,0.0,0.0 +0,0.548300,0.0,0.0,0.0,0.0,0.0 +0,0.548400,0.0,0.0,0.0,0.0,0.0 +0,0.548500,0.0,0.0,0.0,0.0,0.0 +0,0.548600,0.0,0.0,0.0,0.0,0.0 +0,0.548700,0.0,0.0,0.0,0.0,0.0 +0,0.548800,0.0,0.0,0.0,0.0,0.0 +0,0.548900,0.0,0.0,0.0,0.0,0.0 +0,0.549000,0.0,0.0,0.0,0.0,0.0 +0,0.549100,0.0,0.0,0.0,0.0,0.0 +0,0.549200,0.0,0.0,0.0,0.0,0.0 +0,0.549300,0.0,0.0,0.0,0.0,0.0 +0,0.549400,0.0,0.0,0.0,0.0,0.0 +0,0.549500,0.0,0.0,0.0,0.0,0.0 +0,0.549600,0.0,0.0,0.0,0.0,0.0 +0,0.549700,0.0,0.0,0.0,0.0,0.0 +0,0.549800,0.0,0.0,0.0,0.0,0.0 +0,0.549900,0.0,0.0,0.0,0.0,0.0 +0,0.550000,0.0,0.0,0.0,0.0,0.0 +0,0.550100,0.0,0.0,0.0,0.0,0.0 +0,0.550200,0.0,0.0,0.0,0.0,0.0 +0,0.550300,0.0,0.0,0.0,0.0,0.0 +0,0.550400,0.0,0.0,0.0,0.0,0.0 +0,0.550500,0.0,0.0,0.0,0.0,0.0 +0,0.550600,0.0,0.0,0.0,0.0,0.0 +0,0.550700,0.0,0.0,0.0,0.0,0.0 +0,0.550800,0.0,0.0,0.0,0.0,0.0 +0,0.550900,0.0,0.0,0.0,0.0,0.0 +0,0.551000,0.0,0.0,0.0,0.0,0.0 +0,0.551100,0.0,0.0,0.0,0.0,0.0 +0,0.551200,0.0,0.0,0.0,0.0,0.0 +0,0.551300,0.0,0.0,0.0,0.0,0.0 +0,0.551400,0.0,0.0,0.0,0.0,0.0 +0,0.551500,0.0,0.0,0.0,0.0,0.0 +0,0.551600,0.0,0.0,0.0,0.0,0.0 +0,0.551700,0.0,0.0,0.0,0.0,0.0 +0,0.551800,0.0,0.0,0.0,0.0,0.0 +0,0.551900,0.0,0.0,0.0,0.0,0.0 +0,0.552000,0.0,0.0,0.0,0.0,0.0 +0,0.552100,0.0,0.0,0.0,0.0,0.0 +0,0.552200,0.0,0.0,0.0,0.0,0.0 +0,0.552300,0.0,0.0,0.0,0.0,0.0 +0,0.552400,0.0,0.0,0.0,0.0,0.0 +0,0.552500,0.0,0.0,0.0,0.0,0.0 +0,0.552600,0.0,0.0,0.0,0.0,0.0 +0,0.552700,0.0,0.0,0.0,0.0,0.0 +0,0.552800,0.0,0.0,0.0,0.0,0.0 +0,0.552900,0.0,0.0,0.0,0.0,0.0 +0,0.553000,0.0,0.0,0.0,0.0,0.0 +0,0.553100,0.0,0.0,0.0,0.0,0.0 +0,0.553200,0.0,0.0,0.0,0.0,0.0 +0,0.553300,0.0,0.0,0.0,0.0,0.0 +0,0.553400,0.0,0.0,0.0,0.0,0.0 +0,0.553500,0.0,0.0,0.0,0.0,0.0 +0,0.553600,0.0,0.0,0.0,0.0,0.0 +0,0.553700,0.0,0.0,0.0,0.0,0.0 +0,0.553800,0.0,0.0,0.0,0.0,0.0 +0,0.553900,0.0,0.0,0.0,0.0,0.0 +0,0.554000,0.0,0.0,0.0,0.0,0.0 +0,0.554100,0.0,0.0,0.0,0.0,0.0 +0,0.554200,0.0,0.0,0.0,0.0,0.0 +0,0.554300,0.0,0.0,0.0,0.0,0.0 +0,0.554400,0.0,0.0,0.0,0.0,0.0 +0,0.554500,0.0,0.0,0.0,0.0,0.0 +0,0.554600,0.0,0.0,0.0,0.0,0.0 +0,0.554700,0.0,0.0,0.0,0.0,0.0 +0,0.554800,0.0,0.0,0.0,0.0,0.0 +0,0.554900,0.0,0.0,0.0,0.0,0.0 +0,0.555000,0.0,0.0,0.0,0.0,0.0 +0,0.555100,0.0,0.0,0.0,0.0,0.0 +0,0.555200,0.0,0.0,0.0,0.0,0.0 +0,0.555300,0.0,0.0,0.0,0.0,0.0 +0,0.555400,0.0,0.0,0.0,0.0,0.0 +0,0.555500,0.0,0.0,0.0,0.0,0.0 +0,0.555600,0.0,0.0,0.0,0.0,0.0 +0,0.555700,0.0,0.0,0.0,0.0,0.0 +0,0.555800,0.0,0.0,0.0,0.0,0.0 +0,0.555900,0.0,0.0,0.0,0.0,0.0 +0,0.556000,0.0,0.0,0.0,0.0,0.0 +0,0.556100,0.0,0.0,0.0,0.0,0.0 +0,0.556200,0.0,0.0,0.0,0.0,0.0 +0,0.556300,0.0,0.0,0.0,0.0,0.0 +0,0.556400,0.0,0.0,0.0,0.0,0.0 +0,0.556500,0.0,0.0,0.0,0.0,0.0 +0,0.556600,0.0,0.0,0.0,0.0,0.0 +0,0.556700,0.0,0.0,0.0,0.0,0.0 +0,0.556800,0.0,0.0,0.0,0.0,0.0 +0,0.556900,0.0,0.0,0.0,0.0,0.0 +0,0.557000,0.0,0.0,0.0,0.0,0.0 +0,0.557100,0.0,0.0,0.0,0.0,0.0 +0,0.557200,0.0,0.0,0.0,0.0,0.0 +0,0.557300,0.0,0.0,0.0,0.0,0.0 +0,0.557400,0.0,0.0,0.0,0.0,0.0 +0,0.557500,0.0,0.0,0.0,0.0,0.0 +0,0.557600,0.0,0.0,0.0,0.0,0.0 +0,0.557700,0.0,0.0,0.0,0.0,0.0 +0,0.557800,0.0,0.0,0.0,0.0,0.0 +0,0.557900,0.0,0.0,0.0,0.0,0.0 +0,0.558000,0.0,0.0,0.0,0.0,0.0 +0,0.558100,0.0,0.0,0.0,0.0,0.0 +0,0.558200,0.0,0.0,0.0,0.0,0.0 +0,0.558300,0.0,0.0,0.0,0.0,0.0 +0,0.558400,0.0,0.0,0.0,0.0,0.0 +0,0.558500,0.0,0.0,0.0,0.0,0.0 +0,0.558600,0.0,0.0,0.0,0.0,0.0 +0,0.558700,0.0,0.0,0.0,0.0,0.0 +0,0.558800,0.0,0.0,0.0,0.0,0.0 +0,0.558900,0.0,0.0,0.0,0.0,0.0 +0,0.559000,0.0,0.0,0.0,0.0,0.0 +0,0.559100,0.0,0.0,0.0,0.0,0.0 +0,0.559200,0.0,0.0,0.0,0.0,0.0 +0,0.559300,0.0,0.0,0.0,0.0,0.0 +0,0.559400,0.0,0.0,0.0,0.0,0.0 +0,0.559500,0.0,0.0,0.0,0.0,0.0 +0,0.559600,0.0,0.0,0.0,0.0,0.0 +0,0.559700,0.0,0.0,0.0,0.0,0.0 +0,0.559800,0.0,0.0,0.0,0.0,0.0 +0,0.559900,0.0,0.0,0.0,0.0,0.0 +0,0.560000,0.0,0.0,0.0,0.0,0.0 +0,0.560100,0.0,0.0,0.0,0.0,0.0 +1,73.232149,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.560200,0.0,0.0,0.0,0.0,0.0 +0,0.560300,0.0,0.0,0.0,0.0,0.0 +0,0.560400,0.0,0.0,0.0,0.0,0.0 +0,0.560500,0.0,0.0,0.0,0.0,0.0 +0,0.560600,0.0,0.0,0.0,0.0,0.0 +0,0.560700,0.0,0.0,0.0,0.0,0.0 +0,0.560800,0.0,0.0,0.0,0.0,0.0 +0,0.560900,0.0,0.0,0.0,0.0,0.0 +0,0.561000,0.0,0.0,0.0,0.0,0.0 +0,0.561100,0.0,0.0,0.0,0.0,0.0 +0,0.561200,0.0,0.0,0.0,0.0,0.0 +0,0.561300,0.0,0.0,0.0,0.0,0.0 +0,0.561400,0.0,0.0,0.0,0.0,0.0 +0,0.561500,0.0,0.0,0.0,0.0,0.0 +0,0.561600,0.0,0.0,0.0,0.0,0.0 +0,0.561700,0.0,0.0,0.0,0.0,0.0 +0,0.561800,0.0,0.0,0.0,0.0,0.0 +0,0.561900,0.0,0.0,0.0,0.0,0.0 +0,0.562000,0.0,0.0,0.0,0.0,0.0 +0,0.562100,0.0,0.0,0.0,0.0,0.0 +0,0.562200,0.0,0.0,0.0,0.0,0.0 +0,0.562300,0.0,0.0,0.0,0.0,0.0 +0,0.562400,0.0,0.0,0.0,0.0,0.0 +0,0.562500,0.0,0.0,0.0,0.0,0.0 +0,0.562600,0.0,0.0,0.0,0.0,0.0 +0,0.562700,0.0,0.0,0.0,0.0,0.0 +0,0.562800,0.0,0.0,0.0,0.0,0.0 +0,0.562900,0.0,0.0,0.0,0.0,0.0 +0,0.563000,0.0,0.0,0.0,0.0,0.0 +0,0.563100,0.0,0.0,0.0,0.0,0.0 +0,0.563200,0.0,0.0,0.0,0.0,0.0 +0,0.563300,0.0,0.0,0.0,0.0,0.0 +0,0.563400,0.0,0.0,0.0,0.0,0.0 +0,0.563500,0.0,0.0,0.0,0.0,0.0 +0,0.563600,0.0,0.0,0.0,0.0,0.0 +0,0.563700,0.0,0.0,0.0,0.0,0.0 +0,0.563800,0.0,0.0,0.0,0.0,0.0 +0,0.563900,0.0,0.0,0.0,0.0,0.0 +0,0.564000,0.0,0.0,0.0,0.0,0.0 +0,0.564100,0.0,0.0,0.0,0.0,0.0 +0,0.564200,0.0,0.0,0.0,0.0,0.0 +0,0.564300,0.0,0.0,0.0,0.0,0.0 +0,0.564400,0.0,0.0,0.0,0.0,0.0 +0,0.564500,0.0,0.0,0.0,0.0,0.0 +0,0.564600,0.0,0.0,0.0,0.0,0.0 +0,0.564700,0.0,0.0,0.0,0.0,0.0 +0,0.564800,0.0,0.0,0.0,0.0,0.0 +0,0.564900,0.0,0.0,0.0,0.0,0.0 +0,0.565000,0.0,0.0,0.0,0.0,0.0 +0,0.565100,0.0,0.0,0.0,0.0,0.0 +0,0.565200,0.0,0.0,0.0,0.0,0.0 +0,0.565300,0.0,0.0,0.0,0.0,0.0 +0,0.565400,0.0,0.0,0.0,0.0,0.0 +0,0.565500,0.0,0.0,0.0,0.0,0.0 +0,0.565600,0.0,0.0,0.0,0.0,0.0 +0,0.565700,0.0,0.0,0.0,0.0,0.0 +0,0.565800,0.0,0.0,0.0,0.0,0.0 +0,0.565900,0.0,0.0,0.0,0.0,0.0 +0,0.566000,0.0,0.0,0.0,0.0,0.0 +0,0.566100,0.0,0.0,0.0,0.0,0.0 +0,0.566200,0.0,0.0,0.0,0.0,0.0 +0,0.566300,0.0,0.0,0.0,0.0,0.0 +0,0.566400,0.0,0.0,0.0,0.0,0.0 +0,0.566500,0.0,0.0,0.0,0.0,0.0 +0,0.566600,0.0,0.0,0.0,0.0,0.0 +0,0.566700,0.0,0.0,0.0,0.0,0.0 +0,0.566800,0.0,0.0,0.0,0.0,0.0 +0,0.566900,0.0,0.0,0.0,0.0,0.0 +0,0.567000,0.0,0.0,0.0,0.0,0.0 +0,0.567100,0.0,0.0,0.0,0.0,0.0 +0,0.567200,0.0,0.0,0.0,0.0,0.0 +0,0.567300,0.0,0.0,0.0,0.0,0.0 +0,0.567400,0.0,0.0,0.0,0.0,0.0 +0,0.567500,0.0,0.0,0.0,0.0,0.0 +0,0.567600,0.0,0.0,0.0,0.0,0.0 +0,0.567700,0.0,0.0,0.0,0.0,0.0 +0,0.567800,0.0,0.0,0.0,0.0,0.0 +0,0.567900,0.0,0.0,0.0,0.0,0.0 +0,0.568000,0.0,0.0,0.0,0.0,0.0 +0,0.568100,0.0,0.0,0.0,0.0,0.0 +0,0.568200,0.0,0.0,0.0,0.0,0.0 +0,0.568300,0.0,0.0,0.0,0.0,0.0 +0,0.568400,0.0,0.0,0.0,0.0,0.0 +0,0.568500,0.0,0.0,0.0,0.0,0.0 +0,0.568600,0.0,0.0,0.0,0.0,0.0 +0,0.568700,0.0,0.0,0.0,0.0,0.0 +0,0.568800,0.0,0.0,0.0,0.0,0.0 +0,0.568900,0.0,0.0,0.0,0.0,0.0 +0,0.569000,0.0,0.0,0.0,0.0,0.0 +0,0.569100,0.0,0.0,0.0,0.0,0.0 +0,0.569200,0.0,0.0,0.0,0.0,0.0 +0,0.569300,0.0,0.0,0.0,0.0,0.0 +0,0.569400,0.0,0.0,0.0,0.0,0.0 +0,0.569500,0.0,0.0,0.0,0.0,0.0 +0,0.569600,0.0,0.0,0.0,0.0,0.0 +0,0.569700,0.0,0.0,0.0,0.0,0.0 +0,0.569800,0.0,0.0,0.0,0.0,0.0 +0,0.569900,0.0,0.0,0.0,0.0,0.0 +0,0.570000,0.0,0.0,0.0,0.0,0.0 +0,0.570100,0.0,0.0,0.0,0.0,0.0 +0,0.570200,0.0,0.0,0.0,0.0,0.0 +0,0.570300,0.0,0.0,0.0,0.0,0.0 +0,0.570400,0.0,0.0,0.0,0.0,0.0 +0,0.570500,0.0,0.0,0.0,0.0,0.0 +0,0.570600,0.0,0.0,0.0,0.0,0.0 +0,0.570700,0.0,0.0,0.0,0.0,0.0 +0,0.570800,0.0,0.0,0.0,0.0,0.0 +0,0.570900,0.0,0.0,0.0,0.0,0.0 +0,0.571000,0.0,0.0,0.0,0.0,0.0 +0,0.571100,0.0,0.0,0.0,0.0,0.0 +0,0.571200,0.0,0.0,0.0,0.0,0.0 +0,0.571300,0.0,0.0,0.0,0.0,0.0 +0,0.571400,0.0,0.0,0.0,0.0,0.0 +0,0.571500,0.0,0.0,0.0,0.0,0.0 +0,0.571600,0.0,0.0,0.0,0.0,0.0 +0,0.571700,0.0,0.0,0.0,0.0,0.0 +0,0.571800,0.0,0.0,0.0,0.0,0.0 +0,0.571900,0.0,0.0,0.0,0.0,0.0 +0,0.572000,0.0,0.0,0.0,0.0,0.0 +0,0.572100,0.0,0.0,0.0,0.0,0.0 +0,0.572200,0.0,0.0,0.0,0.0,0.0 +0,0.572300,0.0,0.0,0.0,0.0,0.0 +0,0.572400,0.0,0.0,0.0,0.0,0.0 +0,0.572500,0.0,0.0,0.0,0.0,0.0 +0,0.572600,0.0,0.0,0.0,0.0,0.0 +0,0.572700,0.0,0.0,0.0,0.0,0.0 +0,0.572800,0.0,0.0,0.0,0.0,0.0 +0,0.572900,0.0,0.0,0.0,0.0,0.0 +0,0.573000,0.0,0.0,0.0,0.0,0.0 +0,0.573100,0.0,0.0,0.0,0.0,0.0 +0,0.573200,0.0,0.0,0.0,0.0,0.0 +0,0.573300,0.0,0.0,0.0,0.0,0.0 +0,0.573400,0.0,0.0,0.0,0.0,0.0 +0,0.573500,0.0,0.0,0.0,0.0,0.0 +0,0.573600,0.0,0.0,0.0,0.0,0.0 +0,0.573700,0.0,0.0,0.0,0.0,0.0 +0,0.573800,0.0,0.0,0.0,0.0,0.0 +0,0.573900,0.0,0.0,0.0,0.0,0.0 +0,0.574000,0.0,0.0,0.0,0.0,0.0 +0,0.574100,0.0,0.0,0.0,0.0,0.0 +0,0.574200,0.0,0.0,0.0,0.0,0.0 +0,0.574300,0.0,0.0,0.0,0.0,0.0 +0,0.574400,0.0,0.0,0.0,0.0,0.0 +0,0.574500,0.0,0.0,0.0,0.0,0.0 +0,0.574600,0.0,0.0,0.0,0.0,0.0 +0,0.574700,0.0,0.0,0.0,0.0,0.0 +0,0.574800,0.0,0.0,0.0,0.0,0.0 +0,0.574900,0.0,0.0,0.0,0.0,0.0 +0,0.575000,0.0,0.0,0.0,0.0,0.0 +0,0.575100,0.0,0.0,0.0,0.0,0.0 +0,0.575200,0.0,0.0,0.0,0.0,0.0 +0,0.575300,0.0,0.0,0.0,0.0,0.0 +0,0.575400,0.0,0.0,0.0,0.0,0.0 +0,0.575500,0.0,0.0,0.0,0.0,0.0 +0,0.575600,0.0,0.0,0.0,0.0,0.0 +0,0.575700,0.0,0.0,0.0,0.0,0.0 +0,0.575800,0.0,0.0,0.0,0.0,0.0 +0,0.575900,0.0,0.0,0.0,0.0,0.0 +0,0.576000,0.0,0.0,0.0,0.0,0.0 +0,0.576100,0.0,0.0,0.0,0.0,0.0 +0,0.576200,0.0,0.0,0.0,0.0,0.0 +0,0.576300,0.0,0.0,0.0,0.0,0.0 +0,0.576400,0.0,0.0,0.0,0.0,0.0 +0,0.576500,0.0,0.0,0.0,0.0,0.0 +0,0.576600,0.0,0.0,0.0,0.0,0.0 +0,0.576700,0.0,0.0,0.0,0.0,0.0 +0,0.576800,0.0,0.0,0.0,0.0,0.0 +0,0.576900,0.0,0.0,0.0,0.0,0.0 +0,0.577000,0.0,0.0,0.0,0.0,0.0 +0,0.577100,0.0,0.0,0.0,0.0,0.0 +0,0.577200,0.0,0.0,0.0,0.0,0.0 +0,0.577300,0.0,0.0,0.0,0.0,0.0 +0,0.577400,0.0,0.0,0.0,0.0,0.0 +0,0.577500,0.0,0.0,0.0,0.0,0.0 +0,0.577600,0.0,0.0,0.0,0.0,0.0 +0,0.577700,0.0,0.0,0.0,0.0,0.0 +0,0.577800,0.0,0.0,0.0,0.0,0.0 +0,0.577900,0.0,0.0,0.0,0.0,0.0 +0,0.578000,0.0,0.0,0.0,0.0,0.0 +0,0.578100,0.0,0.0,0.0,0.0,0.0 +0,0.578200,0.0,0.0,0.0,0.0,0.0 +0,0.578300,0.0,0.0,0.0,0.0,0.0 +0,0.578400,0.0,0.0,0.0,0.0,0.0 +0,0.578500,0.0,0.0,0.0,0.0,0.0 +0,0.578600,0.0,0.0,0.0,0.0,0.0 +0,0.578700,0.0,0.0,0.0,0.0,0.0 +0,0.578800,0.0,0.0,0.0,0.0,0.0 +0,0.578900,0.0,0.0,0.0,0.0,0.0 +0,0.579000,0.0,0.0,0.0,0.0,0.0 +0,0.579100,0.0,0.0,0.0,0.0,0.0 +0,0.579200,0.0,0.0,0.0,0.0,0.0 +0,0.579300,0.0,0.0,0.0,0.0,0.0 +0,0.579400,0.0,0.0,0.0,0.0,0.0 +0,0.579500,0.0,0.0,0.0,0.0,0.0 +0,0.579600,0.0,0.0,0.0,0.0,0.0 +0,0.579700,0.0,0.0,0.0,0.0,0.0 +0,0.579800,0.0,0.0,0.0,0.0,0.0 +0,0.579900,0.0,0.0,0.0,0.0,0.0 +0,0.580000,0.0,0.0,0.0,0.0,0.0 +0,0.580100,0.0,0.0,0.0,0.0,0.0 +1,81.359757,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.580200,0.0,0.0,0.0,0.0,0.0 +0,0.580300,0.0,0.0,0.0,0.0,0.0 +0,0.580400,0.0,0.0,0.0,0.0,0.0 +0,0.580500,0.0,0.0,0.0,0.0,0.0 +0,0.580600,0.0,0.0,0.0,0.0,0.0 +0,0.580700,0.0,0.0,0.0,0.0,0.0 +0,0.580800,0.0,0.0,0.0,0.0,0.0 +0,0.580900,0.0,0.0,0.0,0.0,0.0 +0,0.581000,0.0,0.0,0.0,0.0,0.0 +0,0.581100,0.0,0.0,0.0,0.0,0.0 +0,0.581200,0.0,0.0,0.0,0.0,0.0 +0,0.581300,0.0,0.0,0.0,0.0,0.0 +0,0.581400,0.0,0.0,0.0,0.0,0.0 +0,0.581500,0.0,0.0,0.0,0.0,0.0 +0,0.581600,0.0,0.0,0.0,0.0,0.0 +0,0.581700,0.0,0.0,0.0,0.0,0.0 +0,0.581800,0.0,0.0,0.0,0.0,0.0 +0,0.581900,0.0,0.0,0.0,0.0,0.0 +0,0.582000,0.0,0.0,0.0,0.0,0.0 +0,0.582100,0.0,0.0,0.0,0.0,0.0 +0,0.582200,0.0,0.0,0.0,0.0,0.0 +0,0.582300,0.0,0.0,0.0,0.0,0.0 +0,0.582400,0.0,0.0,0.0,0.0,0.0 +0,0.582500,0.0,0.0,0.0,0.0,0.0 +0,0.582600,0.0,0.0,0.0,0.0,0.0 +0,0.582700,0.0,0.0,0.0,0.0,0.0 +0,0.582800,0.0,0.0,0.0,0.0,0.0 +0,0.582900,0.0,0.0,0.0,0.0,0.0 +0,0.583000,0.0,0.0,0.0,0.0,0.0 +0,0.583100,0.0,0.0,0.0,0.0,0.0 +0,0.583200,0.0,0.0,0.0,0.0,0.0 +0,0.583300,0.0,0.0,0.0,0.0,0.0 +0,0.583400,0.0,0.0,0.0,0.0,0.0 +0,0.583500,0.0,0.0,0.0,0.0,0.0 +0,0.583600,0.0,0.0,0.0,0.0,0.0 +0,0.583700,0.0,0.0,0.0,0.0,0.0 +0,0.583800,0.0,0.0,0.0,0.0,0.0 +0,0.583900,0.0,0.0,0.0,0.0,0.0 +0,0.584000,0.0,0.0,0.0,0.0,0.0 +0,0.584100,0.0,0.0,0.0,0.0,0.0 +0,0.584200,0.0,0.0,0.0,0.0,0.0 +0,0.584300,0.0,0.0,0.0,0.0,0.0 +0,0.584400,0.0,0.0,0.0,0.0,0.0 +0,0.584500,0.0,0.0,0.0,0.0,0.0 +0,0.584600,0.0,0.0,0.0,0.0,0.0 +0,0.584700,0.0,0.0,0.0,0.0,0.0 +0,0.584800,0.0,0.0,0.0,0.0,0.0 +0,0.584900,0.0,0.0,0.0,0.0,0.0 +0,0.585000,0.0,0.0,0.0,0.0,0.0 +0,0.585100,0.0,0.0,0.0,0.0,0.0 +0,0.585200,0.0,0.0,0.0,0.0,0.0 +0,0.585300,0.0,0.0,0.0,0.0,0.0 +0,0.585400,0.0,0.0,0.0,0.0,0.0 +0,0.585500,0.0,0.0,0.0,0.0,0.0 +0,0.585600,0.0,0.0,0.0,0.0,0.0 +0,0.585700,0.0,0.0,0.0,0.0,0.0 +0,0.585800,0.0,0.0,0.0,0.0,0.0 +0,0.585900,0.0,0.0,0.0,0.0,0.0 +0,0.586000,0.0,0.0,0.0,0.0,0.0 +0,0.586100,0.0,0.0,0.0,0.0,0.0 +0,0.586200,0.0,0.0,0.0,0.0,0.0 +0,0.586300,0.0,0.0,0.0,0.0,0.0 +0,0.586400,0.0,0.0,0.0,0.0,0.0 +0,0.586500,0.0,0.0,0.0,0.0,0.0 +0,0.586600,0.0,0.0,0.0,0.0,0.0 +0,0.586700,0.0,0.0,0.0,0.0,0.0 +0,0.586800,0.0,0.0,0.0,0.0,0.0 +0,0.586900,0.0,0.0,0.0,0.0,0.0 +0,0.587000,0.0,0.0,0.0,0.0,0.0 +0,0.587100,0.0,0.0,0.0,0.0,0.0 +0,0.587200,0.0,0.0,0.0,0.0,0.0 +0,0.587300,0.0,0.0,0.0,0.0,0.0 +0,0.587400,0.0,0.0,0.0,0.0,0.0 +0,0.587500,0.0,0.0,0.0,0.0,0.0 +0,0.587600,0.0,0.0,0.0,0.0,0.0 +0,0.587700,0.0,0.0,0.0,0.0,0.0 +0,0.587800,0.0,0.0,0.0,0.0,0.0 +0,0.587900,0.0,0.0,0.0,0.0,0.0 +0,0.588000,0.0,0.0,0.0,0.0,0.0 +0,0.588100,0.0,0.0,0.0,0.0,0.0 +0,0.588200,0.0,0.0,0.0,0.0,0.0 +0,0.588300,0.0,0.0,0.0,0.0,0.0 +0,0.588400,0.0,0.0,0.0,0.0,0.0 +0,0.588500,0.0,0.0,0.0,0.0,0.0 +0,0.588600,0.0,0.0,0.0,0.0,0.0 +0,0.588700,0.0,0.0,0.0,0.0,0.0 +0,0.588800,0.0,0.0,0.0,0.0,0.0 +0,0.588900,0.0,0.0,0.0,0.0,0.0 +0,0.589000,0.0,0.0,0.0,0.0,0.0 +0,0.589100,0.0,0.0,0.0,0.0,0.0 +0,0.589200,0.0,0.0,0.0,0.0,0.0 +0,0.589300,0.0,0.0,0.0,0.0,0.0 +0,0.589400,0.0,0.0,0.0,0.0,0.0 +0,0.589500,0.0,0.0,0.0,0.0,0.0 +0,0.589600,0.0,0.0,0.0,0.0,0.0 +0,0.589700,0.0,0.0,0.0,0.0,0.0 +0,0.589800,0.0,0.0,0.0,0.0,0.0 +0,0.589900,0.0,0.0,0.0,0.0,0.0 +0,0.590000,0.0,0.0,0.0,0.0,0.0 +0,0.590100,0.0,0.0,0.0,0.0,0.0 +0,0.590200,0.0,0.0,0.0,0.0,0.0 +0,0.590300,0.0,0.0,0.0,0.0,0.0 +0,0.590400,0.0,0.0,0.0,0.0,0.0 +0,0.590500,0.0,0.0,0.0,0.0,0.0 +0,0.590600,0.0,0.0,0.0,0.0,0.0 +0,0.590700,0.0,0.0,0.0,0.0,0.0 +0,0.590800,0.0,0.0,0.0,0.0,0.0 +0,0.590900,0.0,0.0,0.0,0.0,0.0 +0,0.591000,0.0,0.0,0.0,0.0,0.0 +0,0.591100,0.0,0.0,0.0,0.0,0.0 +0,0.591200,0.0,0.0,0.0,0.0,0.0 +0,0.591300,0.0,0.0,0.0,0.0,0.0 +0,0.591400,0.0,0.0,0.0,0.0,0.0 +0,0.591500,0.0,0.0,0.0,0.0,0.0 +0,0.591600,0.0,0.0,0.0,0.0,0.0 +0,0.591700,0.0,0.0,0.0,0.0,0.0 +0,0.591800,0.0,0.0,0.0,0.0,0.0 +0,0.591900,0.0,0.0,0.0,0.0,0.0 +0,0.592000,0.0,0.0,0.0,0.0,0.0 +0,0.592100,0.0,0.0,0.0,0.0,0.0 +0,0.592200,0.0,0.0,0.0,0.0,0.0 +0,0.592300,0.0,0.0,0.0,0.0,0.0 +0,0.592400,0.0,0.0,0.0,0.0,0.0 +0,0.592500,0.0,0.0,0.0,0.0,0.0 +0,0.592600,0.0,0.0,0.0,0.0,0.0 +0,0.592700,0.0,0.0,0.0,0.0,0.0 +0,0.592800,0.0,0.0,0.0,0.0,0.0 +0,0.592900,0.0,0.0,0.0,0.0,0.0 +0,0.593000,0.0,0.0,0.0,0.0,0.0 +0,0.593100,0.0,0.0,0.0,0.0,0.0 +0,0.593200,0.0,0.0,0.0,0.0,0.0 +0,0.593300,0.0,0.0,0.0,0.0,0.0 +0,0.593400,0.0,0.0,0.0,0.0,0.0 +0,0.593500,0.0,0.0,0.0,0.0,0.0 +0,0.593600,0.0,0.0,0.0,0.0,0.0 +0,0.593700,0.0,0.0,0.0,0.0,0.0 +0,0.593800,0.0,0.0,0.0,0.0,0.0 +0,0.593900,0.0,0.0,0.0,0.0,0.0 +0,0.594000,0.0,0.0,0.0,0.0,0.0 +0,0.594100,0.0,0.0,0.0,0.0,0.0 +0,0.594200,0.0,0.0,0.0,0.0,0.0 +0,0.594300,0.0,0.0,0.0,0.0,0.0 +0,0.594400,0.0,0.0,0.0,0.0,0.0 +0,0.594500,0.0,0.0,0.0,0.0,0.0 +0,0.594600,0.0,0.0,0.0,0.0,0.0 +0,0.594700,0.0,0.0,0.0,0.0,0.0 +0,0.594800,0.0,0.0,0.0,0.0,0.0 +0,0.594900,0.0,0.0,0.0,0.0,0.0 +0,0.595000,0.0,0.0,0.0,0.0,0.0 +0,0.595100,0.0,0.0,0.0,0.0,0.0 +0,0.595200,0.0,0.0,0.0,0.0,0.0 +0,0.595300,0.0,0.0,0.0,0.0,0.0 +0,0.595400,0.0,0.0,0.0,0.0,0.0 +0,0.595500,0.0,0.0,0.0,0.0,0.0 +0,0.595600,0.0,0.0,0.0,0.0,0.0 +0,0.595700,0.0,0.0,0.0,0.0,0.0 +0,0.595800,0.0,0.0,0.0,0.0,0.0 +0,0.595900,0.0,0.0,0.0,0.0,0.0 +0,0.596000,0.0,0.0,0.0,0.0,0.0 +0,0.596100,0.0,0.0,0.0,0.0,0.0 +0,0.596200,0.0,0.0,0.0,0.0,0.0 +0,0.596300,0.0,0.0,0.0,0.0,0.0 +0,0.596400,0.0,0.0,0.0,0.0,0.0 +0,0.596500,0.0,0.0,0.0,0.0,0.0 +0,0.596600,0.0,0.0,0.0,0.0,0.0 +0,0.596700,0.0,0.0,0.0,0.0,0.0 +0,0.596800,0.0,0.0,0.0,0.0,0.0 +0,0.596900,0.0,0.0,0.0,0.0,0.0 +0,0.597000,0.0,0.0,0.0,0.0,0.0 +0,0.597100,0.0,0.0,0.0,0.0,0.0 +0,0.597200,0.0,0.0,0.0,0.0,0.0 +0,0.597300,0.0,0.0,0.0,0.0,0.0 +0,0.597400,0.0,0.0,0.0,0.0,0.0 +0,0.597500,0.0,0.0,0.0,0.0,0.0 +0,0.597600,0.0,0.0,0.0,0.0,0.0 +0,0.597700,0.0,0.0,0.0,0.0,0.0 +0,0.597800,0.0,0.0,0.0,0.0,0.0 +0,0.597900,0.0,0.0,0.0,0.0,0.0 +0,0.598000,0.0,0.0,0.0,0.0,0.0 +0,0.598100,0.0,0.0,0.0,0.0,0.0 +0,0.598200,0.0,0.0,0.0,0.0,0.0 +0,0.598300,0.0,0.0,0.0,0.0,0.0 +0,0.598400,0.0,0.0,0.0,0.0,0.0 +0,0.598500,0.0,0.0,0.0,0.0,0.0 +0,0.598600,0.0,0.0,0.0,0.0,0.0 +0,0.598700,0.0,0.0,0.0,0.0,0.0 +0,0.598800,0.0,0.0,0.0,0.0,0.0 +0,0.598900,0.0,0.0,0.0,0.0,0.0 +0,0.599000,0.0,0.0,0.0,0.0,0.0 +0,0.599100,0.0,0.0,0.0,0.0,0.0 +0,0.599200,0.0,0.0,0.0,0.0,0.0 +0,0.599300,0.0,0.0,0.0,0.0,0.0 +0,0.599400,0.0,0.0,0.0,0.0,0.0 +0,0.599500,0.0,0.0,0.0,0.0,0.0 +0,0.599600,0.0,0.0,0.0,0.0,0.0 +0,0.599700,0.0,0.0,0.0,0.0,0.0 +0,0.599800,0.0,0.0,0.0,0.0,0.0 +0,0.599900,0.0,0.0,0.0,0.0,0.0 +0,0.600000,0.0,0.0,0.0,0.0,0.0 +0,0.600100,0.0,0.0,0.0,0.0,0.0 +1,90.067516,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.600200,0.0,0.0,0.0,0.0,0.0 +0,0.600300,0.0,0.0,0.0,0.0,0.0 +0,0.600400,0.0,0.0,0.0,0.0,0.0 +0,0.600500,0.0,0.0,0.0,0.0,0.0 +0,0.600600,0.0,0.0,0.0,0.0,0.0 +0,0.600700,0.0,0.0,0.0,0.0,0.0 +0,0.600800,0.0,0.0,0.0,0.0,0.0 +0,0.600900,0.0,0.0,0.0,0.0,0.0 +0,0.601000,0.0,0.0,0.0,0.0,0.0 +0,0.601100,0.0,0.0,0.0,0.0,0.0 +0,0.601200,0.0,0.0,0.0,0.0,0.0 +0,0.601300,0.0,0.0,0.0,0.0,0.0 +0,0.601400,0.0,0.0,0.0,0.0,0.0 +0,0.601500,0.0,0.0,0.0,0.0,0.0 +0,0.601600,0.0,0.0,0.0,0.0,0.0 +0,0.601700,0.0,0.0,0.0,0.0,0.0 +0,0.601800,0.0,0.0,0.0,0.0,0.0 +0,0.601900,0.0,0.0,0.0,0.0,0.0 +0,0.602000,0.0,0.0,0.0,0.0,0.0 +0,0.602100,0.0,0.0,0.0,0.0,0.0 +0,0.602200,0.0,0.0,0.0,0.0,0.0 +0,0.602300,0.0,0.0,0.0,0.0,0.0 +0,0.602400,0.0,0.0,0.0,0.0,0.0 +0,0.602500,0.0,0.0,0.0,0.0,0.0 +0,0.602600,0.0,0.0,0.0,0.0,0.0 +0,0.602700,0.0,0.0,0.0,0.0,0.0 +0,0.602800,0.0,0.0,0.0,0.0,0.0 +0,0.602900,0.0,0.0,0.0,0.0,0.0 +0,0.603000,0.0,0.0,0.0,0.0,0.0 +0,0.603100,0.0,0.0,0.0,0.0,0.0 +0,0.603200,0.0,0.0,0.0,0.0,0.0 +0,0.603300,0.0,0.0,0.0,0.0,0.0 +0,0.603400,0.0,0.0,0.0,0.0,0.0 +0,0.603500,0.0,0.0,0.0,0.0,0.0 +0,0.603600,0.0,0.0,0.0,0.0,0.0 +0,0.603700,0.0,0.0,0.0,0.0,0.0 +0,0.603800,0.0,0.0,0.0,0.0,0.0 +0,0.603900,0.0,0.0,0.0,0.0,0.0 +0,0.604000,0.0,0.0,0.0,0.0,0.0 +0,0.604100,0.0,0.0,0.0,0.0,0.0 +0,0.604200,0.0,0.0,0.0,0.0,0.0 +0,0.604300,0.0,0.0,0.0,0.0,0.0 +0,0.604400,0.0,0.0,0.0,0.0,0.0 +0,0.604500,0.0,0.0,0.0,0.0,0.0 +0,0.604600,0.0,0.0,0.0,0.0,0.0 +0,0.604700,0.0,0.0,0.0,0.0,0.0 +0,0.604800,0.0,0.0,0.0,0.0,0.0 +0,0.604900,0.0,0.0,0.0,0.0,0.0 +0,0.605000,0.0,0.0,0.0,0.0,0.0 +0,0.605100,0.0,0.0,0.0,0.0,0.0 +0,0.605200,0.0,0.0,0.0,0.0,0.0 +0,0.605300,0.0,0.0,0.0,0.0,0.0 +0,0.605400,0.0,0.0,0.0,0.0,0.0 +0,0.605500,0.0,0.0,0.0,0.0,0.0 +0,0.605600,0.0,0.0,0.0,0.0,0.0 +0,0.605700,0.0,0.0,0.0,0.0,0.0 +0,0.605800,0.0,0.0,0.0,0.0,0.0 +0,0.605900,0.0,0.0,0.0,0.0,0.0 +0,0.606000,0.0,0.0,0.0,0.0,0.0 +0,0.606100,0.0,0.0,0.0,0.0,0.0 +0,0.606200,0.0,0.0,0.0,0.0,0.0 +0,0.606300,0.0,0.0,0.0,0.0,0.0 +0,0.606400,0.0,0.0,0.0,0.0,0.0 +0,0.606500,0.0,0.0,0.0,0.0,0.0 +0,0.606600,0.0,0.0,0.0,0.0,0.0 +0,0.606700,0.0,0.0,0.0,0.0,0.0 +0,0.606800,0.0,0.0,0.0,0.0,0.0 +0,0.606900,0.0,0.0,0.0,0.0,0.0 +0,0.607000,0.0,0.0,0.0,0.0,0.0 +0,0.607100,0.0,0.0,0.0,0.0,0.0 +0,0.607200,0.0,0.0,0.0,0.0,0.0 +0,0.607300,0.0,0.0,0.0,0.0,0.0 +0,0.607400,0.0,0.0,0.0,0.0,0.0 +0,0.607500,0.0,0.0,0.0,0.0,0.0 +0,0.607600,0.0,0.0,0.0,0.0,0.0 +0,0.607700,0.0,0.0,0.0,0.0,0.0 +0,0.607800,0.0,0.0,0.0,0.0,0.0 +0,0.607900,0.0,0.0,0.0,0.0,0.0 +0,0.608000,0.0,0.0,0.0,0.0,0.0 +0,0.608100,0.0,0.0,0.0,0.0,0.0 +0,0.608200,0.0,0.0,0.0,0.0,0.0 +0,0.608300,0.0,0.0,0.0,0.0,0.0 +0,0.608400,0.0,0.0,0.0,0.0,0.0 +0,0.608500,0.0,0.0,0.0,0.0,0.0 +0,0.608600,0.0,0.0,0.0,0.0,0.0 +0,0.608700,0.0,0.0,0.0,0.0,0.0 +0,0.608800,0.0,0.0,0.0,0.0,0.0 +0,0.608900,0.0,0.0,0.0,0.0,0.0 +0,0.609000,0.0,0.0,0.0,0.0,0.0 +0,0.609100,0.0,0.0,0.0,0.0,0.0 +0,0.609200,0.0,0.0,0.0,0.0,0.0 +0,0.609300,0.0,0.0,0.0,0.0,0.0 +0,0.609400,0.0,0.0,0.0,0.0,0.0 +0,0.609500,0.0,0.0,0.0,0.0,0.0 +0,0.609600,0.0,0.0,0.0,0.0,0.0 +0,0.609700,0.0,0.0,0.0,0.0,0.0 +0,0.609800,0.0,0.0,0.0,0.0,0.0 +0,0.609900,0.0,0.0,0.0,0.0,0.0 +0,0.610000,0.0,0.0,0.0,0.0,0.0 +0,0.610100,0.0,0.0,0.0,0.0,0.0 +0,0.610200,0.0,0.0,0.0,0.0,0.0 +0,0.610300,0.0,0.0,0.0,0.0,0.0 +0,0.610400,0.0,0.0,0.0,0.0,0.0 +0,0.610500,0.0,0.0,0.0,0.0,0.0 +0,0.610600,0.0,0.0,0.0,0.0,0.0 +0,0.610700,0.0,0.0,0.0,0.0,0.0 +0,0.610800,0.0,0.0,0.0,0.0,0.0 +0,0.610900,0.0,0.0,0.0,0.0,0.0 +0,0.611000,0.0,0.0,0.0,0.0,0.0 +0,0.611100,0.0,0.0,0.0,0.0,0.0 +0,0.611200,0.0,0.0,0.0,0.0,0.0 +0,0.611300,0.0,0.0,0.0,0.0,0.0 +0,0.611400,0.0,0.0,0.0,0.0,0.0 +0,0.611500,0.0,0.0,0.0,0.0,0.0 +0,0.611600,0.0,0.0,0.0,0.0,0.0 +0,0.611700,0.0,0.0,0.0,0.0,0.0 +0,0.611800,0.0,0.0,0.0,0.0,0.0 +0,0.611900,0.0,0.0,0.0,0.0,0.0 +0,0.612000,0.0,0.0,0.0,0.0,0.0 +0,0.612100,0.0,0.0,0.0,0.0,0.0 +0,0.612200,0.0,0.0,0.0,0.0,0.0 +0,0.612300,0.0,0.0,0.0,0.0,0.0 +0,0.612400,0.0,0.0,0.0,0.0,0.0 +0,0.612500,0.0,0.0,0.0,0.0,0.0 +0,0.612600,0.0,0.0,0.0,0.0,0.0 +0,0.612700,0.0,0.0,0.0,0.0,0.0 +0,0.612800,0.0,0.0,0.0,0.0,0.0 +0,0.612900,0.0,0.0,0.0,0.0,0.0 +0,0.613000,0.0,0.0,0.0,0.0,0.0 +0,0.613100,0.0,0.0,0.0,0.0,0.0 +0,0.613200,0.0,0.0,0.0,0.0,0.0 +0,0.613300,0.0,0.0,0.0,0.0,0.0 +0,0.613400,0.0,0.0,0.0,0.0,0.0 +0,0.613500,0.0,0.0,0.0,0.0,0.0 +0,0.613600,0.0,0.0,0.0,0.0,0.0 +0,0.613700,0.0,0.0,0.0,0.0,0.0 +0,0.613800,0.0,0.0,0.0,0.0,0.0 +0,0.613900,0.0,0.0,0.0,0.0,0.0 +0,0.614000,0.0,0.0,0.0,0.0,0.0 +0,0.614100,0.0,0.0,0.0,0.0,0.0 +0,0.614200,0.0,0.0,0.0,0.0,0.0 +0,0.614300,0.0,0.0,0.0,0.0,0.0 +0,0.614400,0.0,0.0,0.0,0.0,0.0 +0,0.614500,0.0,0.0,0.0,0.0,0.0 +0,0.614600,0.0,0.0,0.0,0.0,0.0 +0,0.614700,0.0,0.0,0.0,0.0,0.0 +0,0.614800,0.0,0.0,0.0,0.0,0.0 +0,0.614900,0.0,0.0,0.0,0.0,0.0 +0,0.615000,0.0,0.0,0.0,0.0,0.0 +0,0.615100,0.0,0.0,0.0,0.0,0.0 +0,0.615200,0.0,0.0,0.0,0.0,0.0 +0,0.615300,0.0,0.0,0.0,0.0,0.0 +0,0.615400,0.0,0.0,0.0,0.0,0.0 +0,0.615500,0.0,0.0,0.0,0.0,0.0 +0,0.615600,0.0,0.0,0.0,0.0,0.0 +0,0.615700,0.0,0.0,0.0,0.0,0.0 +0,0.615800,0.0,0.0,0.0,0.0,0.0 +0,0.615900,0.0,0.0,0.0,0.0,0.0 +0,0.616000,0.0,0.0,0.0,0.0,0.0 +0,0.616100,0.0,0.0,0.0,0.0,0.0 +0,0.616200,0.0,0.0,0.0,0.0,0.0 +0,0.616300,0.0,0.0,0.0,0.0,0.0 +0,0.616400,0.0,0.0,0.0,0.0,0.0 +0,0.616500,0.0,0.0,0.0,0.0,0.0 +0,0.616600,0.0,0.0,0.0,0.0,0.0 +0,0.616700,0.0,0.0,0.0,0.0,0.0 +0,0.616800,0.0,0.0,0.0,0.0,0.0 +0,0.616900,0.0,0.0,0.0,0.0,0.0 +0,0.617000,0.0,0.0,0.0,0.0,0.0 +0,0.617100,0.0,0.0,0.0,0.0,0.0 +0,0.617200,0.0,0.0,0.0,0.0,0.0 +0,0.617300,0.0,0.0,0.0,0.0,0.0 +0,0.617400,0.0,0.0,0.0,0.0,0.0 +0,0.617500,0.0,0.0,0.0,0.0,0.0 +0,0.617600,0.0,0.0,0.0,0.0,0.0 +0,0.617700,0.0,0.0,0.0,0.0,0.0 +0,0.617800,0.0,0.0,0.0,0.0,0.0 +0,0.617900,0.0,0.0,0.0,0.0,0.0 +0,0.618000,0.0,0.0,0.0,0.0,0.0 +0,0.618100,0.0,0.0,0.0,0.0,0.0 +0,0.618200,0.0,0.0,0.0,0.0,0.0 +0,0.618300,0.0,0.0,0.0,0.0,0.0 +0,0.618400,0.0,0.0,0.0,0.0,0.0 +0,0.618500,0.0,0.0,0.0,0.0,0.0 +0,0.618600,0.0,0.0,0.0,0.0,0.0 +0,0.618700,0.0,0.0,0.0,0.0,0.0 +0,0.618800,0.0,0.0,0.0,0.0,0.0 +0,0.618900,0.0,0.0,0.0,0.0,0.0 +0,0.619000,0.0,0.0,0.0,0.0,0.0 +0,0.619100,0.0,0.0,0.0,0.0,0.0 +0,0.619200,0.0,0.0,0.0,0.0,0.0 +0,0.619300,0.0,0.0,0.0,0.0,0.0 +0,0.619400,0.0,0.0,0.0,0.0,0.0 +0,0.619500,0.0,0.0,0.0,0.0,0.0 +0,0.619600,0.0,0.0,0.0,0.0,0.0 +0,0.619700,0.0,0.0,0.0,0.0,0.0 +0,0.619800,0.0,0.0,0.0,0.0,0.0 +0,0.619900,0.0,0.0,0.0,0.0,0.0 +0,0.620000,0.0,0.0,0.0,0.0,0.0 +0,0.620100,0.0,0.0,0.0,0.0,0.0 +1,99.375425,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.620200,0.0,0.0,0.0,0.0,0.0 +0,0.620300,0.0,0.0,0.0,0.0,0.0 +0,0.620400,0.0,0.0,0.0,0.0,0.0 +0,0.620500,0.0,0.0,0.0,0.0,0.0 +0,0.620600,0.0,0.0,0.0,0.0,0.0 +0,0.620700,0.0,0.0,0.0,0.0,0.0 +0,0.620800,0.0,0.0,0.0,0.0,0.0 +0,0.620900,0.0,0.0,0.0,0.0,0.0 +0,0.621000,0.0,0.0,0.0,0.0,0.0 +0,0.621100,0.0,0.0,0.0,0.0,0.0 +0,0.621200,0.0,0.0,0.0,0.0,0.0 +0,0.621300,0.0,0.0,0.0,0.0,0.0 +0,0.621400,0.0,0.0,0.0,0.0,0.0 +0,0.621500,0.0,0.0,0.0,0.0,0.0 +0,0.621600,0.0,0.0,0.0,0.0,0.0 +0,0.621700,0.0,0.0,0.0,0.0,0.0 +0,0.621800,0.0,0.0,0.0,0.0,0.0 +0,0.621900,0.0,0.0,0.0,0.0,0.0 +0,0.622000,0.0,0.0,0.0,0.0,0.0 +0,0.622100,0.0,0.0,0.0,0.0,0.0 +0,0.622200,0.0,0.0,0.0,0.0,0.0 +0,0.622300,0.0,0.0,0.0,0.0,0.0 +0,0.622400,0.0,0.0,0.0,0.0,0.0 +0,0.622500,0.0,0.0,0.0,0.0,0.0 +0,0.622600,0.0,0.0,0.0,0.0,0.0 +0,0.622700,0.0,0.0,0.0,0.0,0.0 +0,0.622800,0.0,0.0,0.0,0.0,0.0 +0,0.622900,0.0,0.0,0.0,0.0,0.0 +0,0.623000,0.0,0.0,0.0,0.0,0.0 +0,0.623100,0.0,0.0,0.0,0.0,0.0 +0,0.623200,0.0,0.0,0.0,0.0,0.0 +0,0.623300,0.0,0.0,0.0,0.0,0.0 +0,0.623400,0.0,0.0,0.0,0.0,0.0 +0,0.623500,0.0,0.0,0.0,0.0,0.0 +0,0.623600,0.0,0.0,0.0,0.0,0.0 +0,0.623700,0.0,0.0,0.0,0.0,0.0 +0,0.623800,0.0,0.0,0.0,0.0,0.0 +0,0.623900,0.0,0.0,0.0,0.0,0.0 +0,0.624000,0.0,0.0,0.0,0.0,0.0 +0,0.624100,0.0,0.0,0.0,0.0,0.0 +0,0.624200,0.0,0.0,0.0,0.0,0.0 +0,0.624300,0.0,0.0,0.0,0.0,0.0 +0,0.624400,0.0,0.0,0.0,0.0,0.0 +0,0.624500,0.0,0.0,0.0,0.0,0.0 +0,0.624600,0.0,0.0,0.0,0.0,0.0 +0,0.624700,0.0,0.0,0.0,0.0,0.0 +0,0.624800,0.0,0.0,0.0,0.0,0.0 +0,0.624900,0.0,0.0,0.0,0.0,0.0 +0,0.625000,0.0,0.0,0.0,0.0,0.0 +0,0.625100,0.0,0.0,0.0,0.0,0.0 +0,0.625200,0.0,0.0,0.0,0.0,0.0 +0,0.625300,0.0,0.0,0.0,0.0,0.0 +0,0.625400,0.0,0.0,0.0,0.0,0.0 +0,0.625500,0.0,0.0,0.0,0.0,0.0 +0,0.625600,0.0,0.0,0.0,0.0,0.0 +0,0.625700,0.0,0.0,0.0,0.0,0.0 +0,0.625800,0.0,0.0,0.0,0.0,0.0 +0,0.625900,0.0,0.0,0.0,0.0,0.0 +0,0.626000,0.0,0.0,0.0,0.0,0.0 +0,0.626100,0.0,0.0,0.0,0.0,0.0 +0,0.626200,0.0,0.0,0.0,0.0,0.0 +0,0.626300,0.0,0.0,0.0,0.0,0.0 +0,0.626400,0.0,0.0,0.0,0.0,0.0 +0,0.626500,0.0,0.0,0.0,0.0,0.0 +0,0.626600,0.0,0.0,0.0,0.0,0.0 +0,0.626700,0.0,0.0,0.0,0.0,0.0 +0,0.626800,0.0,0.0,0.0,0.0,0.0 +0,0.626900,0.0,0.0,0.0,0.0,0.0 +0,0.627000,0.0,0.0,0.0,0.0,0.0 +0,0.627100,0.0,0.0,0.0,0.0,0.0 +0,0.627200,0.0,0.0,0.0,0.0,0.0 +0,0.627300,0.0,0.0,0.0,0.0,0.0 +0,0.627400,0.0,0.0,0.0,0.0,0.0 +0,0.627500,0.0,0.0,0.0,0.0,0.0 +0,0.627600,0.0,0.0,0.0,0.0,0.0 +0,0.627700,0.0,0.0,0.0,0.0,0.0 +0,0.627800,0.0,0.0,0.0,0.0,0.0 +0,0.627900,0.0,0.0,0.0,0.0,0.0 +0,0.628000,0.0,0.0,0.0,0.0,0.0 +0,0.628100,0.0,0.0,0.0,0.0,0.0 +0,0.628200,0.0,0.0,0.0,0.0,0.0 +0,0.628300,0.0,0.0,0.0,0.0,0.0 +0,0.628400,0.0,0.0,0.0,0.0,0.0 +0,0.628500,0.0,0.0,0.0,0.0,0.0 +0,0.628600,0.0,0.0,0.0,0.0,0.0 +0,0.628700,0.0,0.0,0.0,0.0,0.0 +0,0.628800,0.0,0.0,0.0,0.0,0.0 +0,0.628900,0.0,0.0,0.0,0.0,0.0 +0,0.629000,0.0,0.0,0.0,0.0,0.0 +0,0.629100,0.0,0.0,0.0,0.0,0.0 +0,0.629200,0.0,0.0,0.0,0.0,0.0 +0,0.629300,0.0,0.0,0.0,0.0,0.0 +0,0.629400,0.0,0.0,0.0,0.0,0.0 +0,0.629500,0.0,0.0,0.0,0.0,0.0 +0,0.629600,0.0,0.0,0.0,0.0,0.0 +0,0.629700,0.0,0.0,0.0,0.0,0.0 +0,0.629800,0.0,0.0,0.0,0.0,0.0 +0,0.629900,0.0,0.0,0.0,0.0,0.0 +0,0.630000,0.0,0.0,0.0,0.0,0.0 +0,0.630100,0.0,0.0,0.0,0.0,0.0 +0,0.630200,0.0,0.0,0.0,0.0,0.0 +0,0.630300,0.0,0.0,0.0,0.0,0.0 +0,0.630400,0.0,0.0,0.0,0.0,0.0 +0,0.630500,0.0,0.0,0.0,0.0,0.0 +0,0.630600,0.0,0.0,0.0,0.0,0.0 +0,0.630700,0.0,0.0,0.0,0.0,0.0 +0,0.630800,0.0,0.0,0.0,0.0,0.0 +0,0.630900,0.0,0.0,0.0,0.0,0.0 +0,0.631000,0.0,0.0,0.0,0.0,0.0 +0,0.631100,0.0,0.0,0.0,0.0,0.0 +0,0.631200,0.0,0.0,0.0,0.0,0.0 +0,0.631300,0.0,0.0,0.0,0.0,0.0 +0,0.631400,0.0,0.0,0.0,0.0,0.0 +0,0.631500,0.0,0.0,0.0,0.0,0.0 +0,0.631600,0.0,0.0,0.0,0.0,0.0 +0,0.631700,0.0,0.0,0.0,0.0,0.0 +0,0.631800,0.0,0.0,0.0,0.0,0.0 +0,0.631900,0.0,0.0,0.0,0.0,0.0 +0,0.632000,0.0,0.0,0.0,0.0,0.0 +0,0.632100,0.0,0.0,0.0,0.0,0.0 +0,0.632200,0.0,0.0,0.0,0.0,0.0 +0,0.632300,0.0,0.0,0.0,0.0,0.0 +0,0.632400,0.0,0.0,0.0,0.0,0.0 +0,0.632500,0.0,0.0,0.0,0.0,0.0 +0,0.632600,0.0,0.0,0.0,0.0,0.0 +0,0.632700,0.0,0.0,0.0,0.0,0.0 +0,0.632800,0.0,0.0,0.0,0.0,0.0 +0,0.632900,0.0,0.0,0.0,0.0,0.0 +0,0.633000,0.0,0.0,0.0,0.0,0.0 +0,0.633100,0.0,0.0,0.0,0.0,0.0 +0,0.633200,0.0,0.0,0.0,0.0,0.0 +0,0.633300,0.0,0.0,0.0,0.0,0.0 +0,0.633400,0.0,0.0,0.0,0.0,0.0 +0,0.633500,0.0,0.0,0.0,0.0,0.0 +0,0.633600,0.0,0.0,0.0,0.0,0.0 +0,0.633700,0.0,0.0,0.0,0.0,0.0 +0,0.633800,0.0,0.0,0.0,0.0,0.0 +0,0.633900,0.0,0.0,0.0,0.0,0.0 +0,0.634000,0.0,0.0,0.0,0.0,0.0 +0,0.634100,0.0,0.0,0.0,0.0,0.0 +0,0.634200,0.0,0.0,0.0,0.0,0.0 +0,0.634300,0.0,0.0,0.0,0.0,0.0 +0,0.634400,0.0,0.0,0.0,0.0,0.0 +0,0.634500,0.0,0.0,0.0,0.0,0.0 +0,0.634600,0.0,0.0,0.0,0.0,0.0 +0,0.634700,0.0,0.0,0.0,0.0,0.0 +0,0.634800,0.0,0.0,0.0,0.0,0.0 +0,0.634900,0.0,0.0,0.0,0.0,0.0 +0,0.635000,0.0,0.0,0.0,0.0,0.0 +0,0.635100,0.0,0.0,0.0,0.0,0.0 +0,0.635200,0.0,0.0,0.0,0.0,0.0 +0,0.635300,0.0,0.0,0.0,0.0,0.0 +0,0.635400,0.0,0.0,0.0,0.0,0.0 +0,0.635500,0.0,0.0,0.0,0.0,0.0 +0,0.635600,0.0,0.0,0.0,0.0,0.0 +0,0.635700,0.0,0.0,0.0,0.0,0.0 +0,0.635800,0.0,0.0,0.0,0.0,0.0 +0,0.635900,0.0,0.0,0.0,0.0,0.0 +0,0.636000,0.0,0.0,0.0,0.0,0.0 +0,0.636100,0.0,0.0,0.0,0.0,0.0 +0,0.636200,0.0,0.0,0.0,0.0,0.0 +0,0.636300,0.0,0.0,0.0,0.0,0.0 +0,0.636400,0.0,0.0,0.0,0.0,0.0 +0,0.636500,0.0,0.0,0.0,0.0,0.0 +0,0.636600,0.0,0.0,0.0,0.0,0.0 +0,0.636700,0.0,0.0,0.0,0.0,0.0 +0,0.636800,0.0,0.0,0.0,0.0,0.0 +0,0.636900,0.0,0.0,0.0,0.0,0.0 +0,0.637000,0.0,0.0,0.0,0.0,0.0 +0,0.637100,0.0,0.0,0.0,0.0,0.0 +0,0.637200,0.0,0.0,0.0,0.0,0.0 +0,0.637300,0.0,0.0,0.0,0.0,0.0 +0,0.637400,0.0,0.0,0.0,0.0,0.0 +0,0.637500,0.0,0.0,0.0,0.0,0.0 +0,0.637600,0.0,0.0,0.0,0.0,0.0 +0,0.637700,0.0,0.0,0.0,0.0,0.0 +0,0.637800,0.0,0.0,0.0,0.0,0.0 +0,0.637900,0.0,0.0,0.0,0.0,0.0 +0,0.638000,0.0,0.0,0.0,0.0,0.0 +0,0.638100,0.0,0.0,0.0,0.0,0.0 +0,0.638200,0.0,0.0,0.0,0.0,0.0 +0,0.638300,0.0,0.0,0.0,0.0,0.0 +0,0.638400,0.0,0.0,0.0,0.0,0.0 +0,0.638500,0.0,0.0,0.0,0.0,0.0 +0,0.638600,0.0,0.0,0.0,0.0,0.0 +0,0.638700,0.0,0.0,0.0,0.0,0.0 +0,0.638800,0.0,0.0,0.0,0.0,0.0 +0,0.638900,0.0,0.0,0.0,0.0,0.0 +0,0.639000,0.0,0.0,0.0,0.0,0.0 +0,0.639100,0.0,0.0,0.0,0.0,0.0 +0,0.639200,0.0,0.0,0.0,0.0,0.0 +0,0.639300,0.0,0.0,0.0,0.0,0.0 +0,0.639400,0.0,0.0,0.0,0.0,0.0 +0,0.639500,0.0,0.0,0.0,0.0,0.0 +0,0.639600,0.0,0.0,0.0,0.0,0.0 +0,0.639700,0.0,0.0,0.0,0.0,0.0 +0,0.639800,0.0,0.0,0.0,0.0,0.0 +0,0.639900,0.0,0.0,0.0,0.0,0.0 +0,0.640000,0.0,0.0,0.0,0.0,0.0 +0,0.640100,0.0,0.0,0.0,0.0,0.0 +1,109.303484,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.640200,0.0,0.0,0.0,0.0,0.0 +0,0.640300,0.0,0.0,0.0,0.0,0.0 +0,0.640400,0.0,0.0,0.0,0.0,0.0 +0,0.640500,0.0,0.0,0.0,0.0,0.0 +0,0.640600,0.0,0.0,0.0,0.0,0.0 +0,0.640700,0.0,0.0,0.0,0.0,0.0 +0,0.640800,0.0,0.0,0.0,0.0,0.0 +0,0.640900,0.0,0.0,0.0,0.0,0.0 +0,0.641000,0.0,0.0,0.0,0.0,0.0 +0,0.641100,0.0,0.0,0.0,0.0,0.0 +0,0.641200,0.0,0.0,0.0,0.0,0.0 +0,0.641300,0.0,0.0,0.0,0.0,0.0 +0,0.641400,0.0,0.0,0.0,0.0,0.0 +0,0.641500,0.0,0.0,0.0,0.0,0.0 +0,0.641600,0.0,0.0,0.0,0.0,0.0 +0,0.641700,0.0,0.0,0.0,0.0,0.0 +0,0.641800,0.0,0.0,0.0,0.0,0.0 +0,0.641900,0.0,0.0,0.0,0.0,0.0 +0,0.642000,0.0,0.0,0.0,0.0,0.0 +0,0.642100,0.0,0.0,0.0,0.0,0.0 +0,0.642200,0.0,0.0,0.0,0.0,0.0 +0,0.642300,0.0,0.0,0.0,0.0,0.0 +0,0.642400,0.0,0.0,0.0,0.0,0.0 +0,0.642500,0.0,0.0,0.0,0.0,0.0 +0,0.642600,0.0,0.0,0.0,0.0,0.0 +0,0.642700,0.0,0.0,0.0,0.0,0.0 +0,0.642800,0.0,0.0,0.0,0.0,0.0 +0,0.642900,0.0,0.0,0.0,0.0,0.0 +0,0.643000,0.0,0.0,0.0,0.0,0.0 +0,0.643100,0.0,0.0,0.0,0.0,0.0 +0,0.643200,0.0,0.0,0.0,0.0,0.0 +0,0.643300,0.0,0.0,0.0,0.0,0.0 +0,0.643400,0.0,0.0,0.0,0.0,0.0 +0,0.643500,0.0,0.0,0.0,0.0,0.0 +0,0.643600,0.0,0.0,0.0,0.0,0.0 +0,0.643700,0.0,0.0,0.0,0.0,0.0 +0,0.643800,0.0,0.0,0.0,0.0,0.0 +0,0.643900,0.0,0.0,0.0,0.0,0.0 +0,0.644000,0.0,0.0,0.0,0.0,0.0 +0,0.644100,0.0,0.0,0.0,0.0,0.0 +0,0.644200,0.0,0.0,0.0,0.0,0.0 +0,0.644300,0.0,0.0,0.0,0.0,0.0 +0,0.644400,0.0,0.0,0.0,0.0,0.0 +0,0.644500,0.0,0.0,0.0,0.0,0.0 +0,0.644600,0.0,0.0,0.0,0.0,0.0 +0,0.644700,0.0,0.0,0.0,0.0,0.0 +0,0.644800,0.0,0.0,0.0,0.0,0.0 +0,0.644900,0.0,0.0,0.0,0.0,0.0 +0,0.645000,0.0,0.0,0.0,0.0,0.0 +0,0.645100,0.0,0.0,0.0,0.0,0.0 +0,0.645200,0.0,0.0,0.0,0.0,0.0 +0,0.645300,0.0,0.0,0.0,0.0,0.0 +0,0.645400,0.0,0.0,0.0,0.0,0.0 +0,0.645500,0.0,0.0,0.0,0.0,0.0 +0,0.645600,0.0,0.0,0.0,0.0,0.0 +0,0.645700,0.0,0.0,0.0,0.0,0.0 +0,0.645800,0.0,0.0,0.0,0.0,0.0 +0,0.645900,0.0,0.0,0.0,0.0,0.0 +0,0.646000,0.0,0.0,0.0,0.0,0.0 +0,0.646100,0.0,0.0,0.0,0.0,0.0 +0,0.646200,0.0,0.0,0.0,0.0,0.0 +0,0.646300,0.0,0.0,0.0,0.0,0.0 +0,0.646400,0.0,0.0,0.0,0.0,0.0 +0,0.646500,0.0,0.0,0.0,0.0,0.0 +0,0.646600,0.0,0.0,0.0,0.0,0.0 +0,0.646700,0.0,0.0,0.0,0.0,0.0 +0,0.646800,0.0,0.0,0.0,0.0,0.0 +0,0.646900,0.0,0.0,0.0,0.0,0.0 +0,0.647000,0.0,0.0,0.0,0.0,0.0 +0,0.647100,0.0,0.0,0.0,0.0,0.0 +0,0.647200,0.0,0.0,0.0,0.0,0.0 +0,0.647300,0.0,0.0,0.0,0.0,0.0 +0,0.647400,0.0,0.0,0.0,0.0,0.0 +0,0.647500,0.0,0.0,0.0,0.0,0.0 +0,0.647600,0.0,0.0,0.0,0.0,0.0 +0,0.647700,0.0,0.0,0.0,0.0,0.0 +0,0.647800,0.0,0.0,0.0,0.0,0.0 +0,0.647900,0.0,0.0,0.0,0.0,0.0 +0,0.648000,0.0,0.0,0.0,0.0,0.0 +0,0.648100,0.0,0.0,0.0,0.0,0.0 +0,0.648200,0.0,0.0,0.0,0.0,0.0 +0,0.648300,0.0,0.0,0.0,0.0,0.0 +0,0.648400,0.0,0.0,0.0,0.0,0.0 +0,0.648500,0.0,0.0,0.0,0.0,0.0 +0,0.648600,0.0,0.0,0.0,0.0,0.0 +0,0.648700,0.0,0.0,0.0,0.0,0.0 +0,0.648800,0.0,0.0,0.0,0.0,0.0 +0,0.648900,0.0,0.0,0.0,0.0,0.0 +0,0.649000,0.0,0.0,0.0,0.0,0.0 +0,0.649100,0.0,0.0,0.0,0.0,0.0 +0,0.649200,0.0,0.0,0.0,0.0,0.0 +0,0.649300,0.0,0.0,0.0,0.0,0.0 +0,0.649400,0.0,0.0,0.0,0.0,0.0 +0,0.649500,0.0,0.0,0.0,0.0,0.0 +0,0.649600,0.0,0.0,0.0,0.0,0.0 +0,0.649700,0.0,0.0,0.0,0.0,0.0 +0,0.649800,0.0,0.0,0.0,0.0,0.0 +0,0.649900,0.0,0.0,0.0,0.0,0.0 +0,0.650000,0.0,0.0,0.0,0.0,0.0 +0,0.650100,0.0,0.0,0.0,0.0,0.0 +0,0.650200,0.0,0.0,0.0,0.0,0.0 +0,0.650300,0.0,0.0,0.0,0.0,0.0 +0,0.650400,0.0,0.0,0.0,0.0,0.0 +0,0.650500,0.0,0.0,0.0,0.0,0.0 +0,0.650600,0.0,0.0,0.0,0.0,0.0 +0,0.650700,0.0,0.0,0.0,0.0,0.0 +0,0.650800,0.0,0.0,0.0,0.0,0.0 +0,0.650900,0.0,0.0,0.0,0.0,0.0 +0,0.651000,0.0,0.0,0.0,0.0,0.0 +0,0.651100,0.0,0.0,0.0,0.0,0.0 +0,0.651200,0.0,0.0,0.0,0.0,0.0 +0,0.651300,0.0,0.0,0.0,0.0,0.0 +0,0.651400,0.0,0.0,0.0,0.0,0.0 +0,0.651500,0.0,0.0,0.0,0.0,0.0 +0,0.651600,0.0,0.0,0.0,0.0,0.0 +0,0.651700,0.0,0.0,0.0,0.0,0.0 +0,0.651800,0.0,0.0,0.0,0.0,0.0 +0,0.651900,0.0,0.0,0.0,0.0,0.0 +0,0.652000,0.0,0.0,0.0,0.0,0.0 +0,0.652100,0.0,0.0,0.0,0.0,0.0 +0,0.652200,0.0,0.0,0.0,0.0,0.0 +0,0.652300,0.0,0.0,0.0,0.0,0.0 +0,0.652400,0.0,0.0,0.0,0.0,0.0 +0,0.652500,0.0,0.0,0.0,0.0,0.0 +0,0.652600,0.0,0.0,0.0,0.0,0.0 +0,0.652700,0.0,0.0,0.0,0.0,0.0 +0,0.652800,0.0,0.0,0.0,0.0,0.0 +0,0.652900,0.0,0.0,0.0,0.0,0.0 +0,0.653000,0.0,0.0,0.0,0.0,0.0 +0,0.653100,0.0,0.0,0.0,0.0,0.0 +0,0.653200,0.0,0.0,0.0,0.0,0.0 +0,0.653300,0.0,0.0,0.0,0.0,0.0 +0,0.653400,0.0,0.0,0.0,0.0,0.0 +0,0.653500,0.0,0.0,0.0,0.0,0.0 +0,0.653600,0.0,0.0,0.0,0.0,0.0 +0,0.653700,0.0,0.0,0.0,0.0,0.0 +0,0.653800,0.0,0.0,0.0,0.0,0.0 +0,0.653900,0.0,0.0,0.0,0.0,0.0 +0,0.654000,0.0,0.0,0.0,0.0,0.0 +0,0.654100,0.0,0.0,0.0,0.0,0.0 +0,0.654200,0.0,0.0,0.0,0.0,0.0 +0,0.654300,0.0,0.0,0.0,0.0,0.0 +0,0.654400,0.0,0.0,0.0,0.0,0.0 +0,0.654500,0.0,0.0,0.0,0.0,0.0 +0,0.654600,0.0,0.0,0.0,0.0,0.0 +0,0.654700,0.0,0.0,0.0,0.0,0.0 +0,0.654800,0.0,0.0,0.0,0.0,0.0 +0,0.654900,0.0,0.0,0.0,0.0,0.0 +0,0.655000,0.0,0.0,0.0,0.0,0.0 +0,0.655100,0.0,0.0,0.0,0.0,0.0 +0,0.655200,0.0,0.0,0.0,0.0,0.0 +0,0.655300,0.0,0.0,0.0,0.0,0.0 +0,0.655400,0.0,0.0,0.0,0.0,0.0 +0,0.655500,0.0,0.0,0.0,0.0,0.0 +0,0.655600,0.0,0.0,0.0,0.0,0.0 +0,0.655700,0.0,0.0,0.0,0.0,0.0 +0,0.655800,0.0,0.0,0.0,0.0,0.0 +0,0.655900,0.0,0.0,0.0,0.0,0.0 +0,0.656000,0.0,0.0,0.0,0.0,0.0 +0,0.656100,0.0,0.0,0.0,0.0,0.0 +0,0.656200,0.0,0.0,0.0,0.0,0.0 +0,0.656300,0.0,0.0,0.0,0.0,0.0 +0,0.656400,0.0,0.0,0.0,0.0,0.0 +0,0.656500,0.0,0.0,0.0,0.0,0.0 +0,0.656600,0.0,0.0,0.0,0.0,0.0 +0,0.656700,0.0,0.0,0.0,0.0,0.0 +0,0.656800,0.0,0.0,0.0,0.0,0.0 +0,0.656900,0.0,0.0,0.0,0.0,0.0 +0,0.657000,0.0,0.0,0.0,0.0,0.0 +0,0.657100,0.0,0.0,0.0,0.0,0.0 +0,0.657200,0.0,0.0,0.0,0.0,0.0 +0,0.657300,0.0,0.0,0.0,0.0,0.0 +0,0.657400,0.0,0.0,0.0,0.0,0.0 +0,0.657500,0.0,0.0,0.0,0.0,0.0 +0,0.657600,0.0,0.0,0.0,0.0,0.0 +0,0.657700,0.0,0.0,0.0,0.0,0.0 +0,0.657800,0.0,0.0,0.0,0.0,0.0 +0,0.657900,0.0,0.0,0.0,0.0,0.0 +0,0.658000,0.0,0.0,0.0,0.0,0.0 +0,0.658100,0.0,0.0,0.0,0.0,0.0 +0,0.658200,0.0,0.0,0.0,0.0,0.0 +0,0.658300,0.0,0.0,0.0,0.0,0.0 +0,0.658400,0.0,0.0,0.0,0.0,0.0 +0,0.658500,0.0,0.0,0.0,0.0,0.0 +0,0.658600,0.0,0.0,0.0,0.0,0.0 +0,0.658700,0.0,0.0,0.0,0.0,0.0 +0,0.658800,0.0,0.0,0.0,0.0,0.0 +0,0.658900,0.0,0.0,0.0,0.0,0.0 +0,0.659000,0.0,0.0,0.0,0.0,0.0 +0,0.659100,0.0,0.0,0.0,0.0,0.0 +0,0.659200,0.0,0.0,0.0,0.0,0.0 +0,0.659300,0.0,0.0,0.0,0.0,0.0 +0,0.659400,0.0,0.0,0.0,0.0,0.0 +0,0.659500,0.0,0.0,0.0,0.0,0.0 +0,0.659600,0.0,0.0,0.0,0.0,0.0 +0,0.659700,0.0,0.0,0.0,0.0,0.0 +0,0.659800,0.0,0.0,0.0,0.0,0.0 +0,0.659900,0.0,0.0,0.0,0.0,0.0 +0,0.660000,0.0,0.0,0.0,0.0,0.0 +0,0.660100,0.0,0.0,0.0,0.0,0.0 +1,119.871693,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.660200,0.0,0.0,0.0,0.0,0.0 +0,0.660300,0.0,0.0,0.0,0.0,0.0 +0,0.660400,0.0,0.0,0.0,0.0,0.0 +0,0.660500,0.0,0.0,0.0,0.0,0.0 +0,0.660600,0.0,0.0,0.0,0.0,0.0 +0,0.660700,0.0,0.0,0.0,0.0,0.0 +0,0.660800,0.0,0.0,0.0,0.0,0.0 +0,0.660900,0.0,0.0,0.0,0.0,0.0 +0,0.661000,0.0,0.0,0.0,0.0,0.0 +0,0.661100,0.0,0.0,0.0,0.0,0.0 +0,0.661200,0.0,0.0,0.0,0.0,0.0 +0,0.661300,0.0,0.0,0.0,0.0,0.0 +0,0.661400,0.0,0.0,0.0,0.0,0.0 +0,0.661500,0.0,0.0,0.0,0.0,0.0 +0,0.661600,0.0,0.0,0.0,0.0,0.0 +0,0.661700,0.0,0.0,0.0,0.0,0.0 +0,0.661800,0.0,0.0,0.0,0.0,0.0 +0,0.661900,0.0,0.0,0.0,0.0,0.0 +0,0.662000,0.0,0.0,0.0,0.0,0.0 +0,0.662100,0.0,0.0,0.0,0.0,0.0 +0,0.662200,0.0,0.0,0.0,0.0,0.0 +0,0.662300,0.0,0.0,0.0,0.0,0.0 +0,0.662400,0.0,0.0,0.0,0.0,0.0 +0,0.662500,0.0,0.0,0.0,0.0,0.0 +0,0.662600,0.0,0.0,0.0,0.0,0.0 +0,0.662700,0.0,0.0,0.0,0.0,0.0 +0,0.662800,0.0,0.0,0.0,0.0,0.0 +0,0.662900,0.0,0.0,0.0,0.0,0.0 +0,0.663000,0.0,0.0,0.0,0.0,0.0 +0,0.663100,0.0,0.0,0.0,0.0,0.0 +0,0.663200,0.0,0.0,0.0,0.0,0.0 +0,0.663300,0.0,0.0,0.0,0.0,0.0 +0,0.663400,0.0,0.0,0.0,0.0,0.0 +0,0.663500,0.0,0.0,0.0,0.0,0.0 +0,0.663600,0.0,0.0,0.0,0.0,0.0 +0,0.663700,0.0,0.0,0.0,0.0,0.0 +0,0.663800,0.0,0.0,0.0,0.0,0.0 +0,0.663900,0.0,0.0,0.0,0.0,0.0 +0,0.664000,0.0,0.0,0.0,0.0,0.0 +0,0.664100,0.0,0.0,0.0,0.0,0.0 +0,0.664200,0.0,0.0,0.0,0.0,0.0 +0,0.664300,0.0,0.0,0.0,0.0,0.0 +0,0.664400,0.0,0.0,0.0,0.0,0.0 +0,0.664500,0.0,0.0,0.0,0.0,0.0 +0,0.664600,0.0,0.0,0.0,0.0,0.0 +0,0.664700,0.0,0.0,0.0,0.0,0.0 +0,0.664800,0.0,0.0,0.0,0.0,0.0 +0,0.664900,0.0,0.0,0.0,0.0,0.0 +0,0.665000,0.0,0.0,0.0,0.0,0.0 +0,0.665100,0.0,0.0,0.0,0.0,0.0 +0,0.665200,0.0,0.0,0.0,0.0,0.0 +0,0.665300,0.0,0.0,0.0,0.0,0.0 +0,0.665400,0.0,0.0,0.0,0.0,0.0 +0,0.665500,0.0,0.0,0.0,0.0,0.0 +0,0.665600,0.0,0.0,0.0,0.0,0.0 +0,0.665700,0.0,0.0,0.0,0.0,0.0 +0,0.665800,0.0,0.0,0.0,0.0,0.0 +0,0.665900,0.0,0.0,0.0,0.0,0.0 +0,0.666000,0.0,0.0,0.0,0.0,0.0 +0,0.666100,0.0,0.0,0.0,0.0,0.0 +0,0.666200,0.0,0.0,0.0,0.0,0.0 +0,0.666300,0.0,0.0,0.0,0.0,0.0 +0,0.666400,0.0,0.0,0.0,0.0,0.0 +0,0.666500,0.0,0.0,0.0,0.0,0.0 +0,0.666600,0.0,0.0,0.0,0.0,0.0 +0,0.666700,0.0,0.0,0.0,0.0,0.0 +0,0.666800,0.0,0.0,0.0,0.0,0.0 +0,0.666900,0.0,0.0,0.0,0.0,0.0 +0,0.667000,0.0,0.0,0.0,0.0,0.0 +0,0.667100,0.0,0.0,0.0,0.0,0.0 +0,0.667200,0.0,0.0,0.0,0.0,0.0 +0,0.667300,0.0,0.0,0.0,0.0,0.0 +0,0.667400,0.0,0.0,0.0,0.0,0.0 +0,0.667500,0.0,0.0,0.0,0.0,0.0 +0,0.667600,0.0,0.0,0.0,0.0,0.0 +0,0.667700,0.0,0.0,0.0,0.0,0.0 +0,0.667800,0.0,0.0,0.0,0.0,0.0 +0,0.667900,0.0,0.0,0.0,0.0,0.0 +0,0.668000,0.0,0.0,0.0,0.0,0.0 +0,0.668100,0.0,0.0,0.0,0.0,0.0 +0,0.668200,0.0,0.0,0.0,0.0,0.0 +0,0.668300,0.0,0.0,0.0,0.0,0.0 +0,0.668400,0.0,0.0,0.0,0.0,0.0 +0,0.668500,0.0,0.0,0.0,0.0,0.0 +0,0.668600,0.0,0.0,0.0,0.0,0.0 +0,0.668700,0.0,0.0,0.0,0.0,0.0 +0,0.668800,0.0,0.0,0.0,0.0,0.0 +0,0.668900,0.0,0.0,0.0,0.0,0.0 +0,0.669000,0.0,0.0,0.0,0.0,0.0 +0,0.669100,0.0,0.0,0.0,0.0,0.0 +0,0.669200,0.0,0.0,0.0,0.0,0.0 +0,0.669300,0.0,0.0,0.0,0.0,0.0 +0,0.669400,0.0,0.0,0.0,0.0,0.0 +0,0.669500,0.0,0.0,0.0,0.0,0.0 +0,0.669600,0.0,0.0,0.0,0.0,0.0 +0,0.669700,0.0,0.0,0.0,0.0,0.0 +0,0.669800,0.0,0.0,0.0,0.0,0.0 +0,0.669900,0.0,0.0,0.0,0.0,0.0 +0,0.670000,0.0,0.0,0.0,0.0,0.0 +0,0.670100,0.0,0.0,0.0,0.0,0.0 +0,0.670200,0.0,0.0,0.0,0.0,0.0 +0,0.670300,0.0,0.0,0.0,0.0,0.0 +0,0.670400,0.0,0.0,0.0,0.0,0.0 +0,0.670500,0.0,0.0,0.0,0.0,0.0 +0,0.670600,0.0,0.0,0.0,0.0,0.0 +0,0.670700,0.0,0.0,0.0,0.0,0.0 +0,0.670800,0.0,0.0,0.0,0.0,0.0 +0,0.670900,0.0,0.0,0.0,0.0,0.0 +0,0.671000,0.0,0.0,0.0,0.0,0.0 +0,0.671100,0.0,0.0,0.0,0.0,0.0 +0,0.671200,0.0,0.0,0.0,0.0,0.0 +0,0.671300,0.0,0.0,0.0,0.0,0.0 +0,0.671400,0.0,0.0,0.0,0.0,0.0 +0,0.671500,0.0,0.0,0.0,0.0,0.0 +0,0.671600,0.0,0.0,0.0,0.0,0.0 +0,0.671700,0.0,0.0,0.0,0.0,0.0 +0,0.671800,0.0,0.0,0.0,0.0,0.0 +0,0.671900,0.0,0.0,0.0,0.0,0.0 +0,0.672000,0.0,0.0,0.0,0.0,0.0 +0,0.672100,0.0,0.0,0.0,0.0,0.0 +0,0.672200,0.0,0.0,0.0,0.0,0.0 +0,0.672300,0.0,0.0,0.0,0.0,0.0 +0,0.672400,0.0,0.0,0.0,0.0,0.0 +0,0.672500,0.0,0.0,0.0,0.0,0.0 +0,0.672600,0.0,0.0,0.0,0.0,0.0 +0,0.672700,0.0,0.0,0.0,0.0,0.0 +0,0.672800,0.0,0.0,0.0,0.0,0.0 +0,0.672900,0.0,0.0,0.0,0.0,0.0 +0,0.673000,0.0,0.0,0.0,0.0,0.0 +0,0.673100,0.0,0.0,0.0,0.0,0.0 +0,0.673200,0.0,0.0,0.0,0.0,0.0 +0,0.673300,0.0,0.0,0.0,0.0,0.0 +0,0.673400,0.0,0.0,0.0,0.0,0.0 +0,0.673500,0.0,0.0,0.0,0.0,0.0 +0,0.673600,0.0,0.0,0.0,0.0,0.0 +0,0.673700,0.0,0.0,0.0,0.0,0.0 +0,0.673800,0.0,0.0,0.0,0.0,0.0 +0,0.673900,0.0,0.0,0.0,0.0,0.0 +0,0.674000,0.0,0.0,0.0,0.0,0.0 +0,0.674100,0.0,0.0,0.0,0.0,0.0 +0,0.674200,0.0,0.0,0.0,0.0,0.0 +0,0.674300,0.0,0.0,0.0,0.0,0.0 +0,0.674400,0.0,0.0,0.0,0.0,0.0 +0,0.674500,0.0,0.0,0.0,0.0,0.0 +0,0.674600,0.0,0.0,0.0,0.0,0.0 +0,0.674700,0.0,0.0,0.0,0.0,0.0 +0,0.674800,0.0,0.0,0.0,0.0,0.0 +0,0.674900,0.0,0.0,0.0,0.0,0.0 +0,0.675000,0.0,0.0,0.0,0.0,0.0 +0,0.675100,0.0,0.0,0.0,0.0,0.0 +0,0.675200,0.0,0.0,0.0,0.0,0.0 +0,0.675300,0.0,0.0,0.0,0.0,0.0 +0,0.675400,0.0,0.0,0.0,0.0,0.0 +0,0.675500,0.0,0.0,0.0,0.0,0.0 +0,0.675600,0.0,0.0,0.0,0.0,0.0 +0,0.675700,0.0,0.0,0.0,0.0,0.0 +0,0.675800,0.0,0.0,0.0,0.0,0.0 +0,0.675900,0.0,0.0,0.0,0.0,0.0 +0,0.676000,0.0,0.0,0.0,0.0,0.0 +0,0.676100,0.0,0.0,0.0,0.0,0.0 +0,0.676200,0.0,0.0,0.0,0.0,0.0 +0,0.676300,0.0,0.0,0.0,0.0,0.0 +0,0.676400,0.0,0.0,0.0,0.0,0.0 +0,0.676500,0.0,0.0,0.0,0.0,0.0 +0,0.676600,0.0,0.0,0.0,0.0,0.0 +0,0.676700,0.0,0.0,0.0,0.0,0.0 +0,0.676800,0.0,0.0,0.0,0.0,0.0 +0,0.676900,0.0,0.0,0.0,0.0,0.0 +0,0.677000,0.0,0.0,0.0,0.0,0.0 +0,0.677100,0.0,0.0,0.0,0.0,0.0 +0,0.677200,0.0,0.0,0.0,0.0,0.0 +0,0.677300,0.0,0.0,0.0,0.0,0.0 +0,0.677400,0.0,0.0,0.0,0.0,0.0 +0,0.677500,0.0,0.0,0.0,0.0,0.0 +0,0.677600,0.0,0.0,0.0,0.0,0.0 +0,0.677700,0.0,0.0,0.0,0.0,0.0 +0,0.677800,0.0,0.0,0.0,0.0,0.0 +0,0.677900,0.0,0.0,0.0,0.0,0.0 +0,0.678000,0.0,0.0,0.0,0.0,0.0 +0,0.678100,0.0,0.0,0.0,0.0,0.0 +0,0.678200,0.0,0.0,0.0,0.0,0.0 +0,0.678300,0.0,0.0,0.0,0.0,0.0 +0,0.678400,0.0,0.0,0.0,0.0,0.0 +0,0.678500,0.0,0.0,0.0,0.0,0.0 +0,0.678600,0.0,0.0,0.0,0.0,0.0 +0,0.678700,0.0,0.0,0.0,0.0,0.0 +0,0.678800,0.0,0.0,0.0,0.0,0.0 +0,0.678900,0.0,0.0,0.0,0.0,0.0 +0,0.679000,0.0,0.0,0.0,0.0,0.0 +0,0.679100,0.0,0.0,0.0,0.0,0.0 +0,0.679200,0.0,0.0,0.0,0.0,0.0 +0,0.679300,0.0,0.0,0.0,0.0,0.0 +0,0.679400,0.0,0.0,0.0,0.0,0.0 +0,0.679500,0.0,0.0,0.0,0.0,0.0 +0,0.679600,0.0,0.0,0.0,0.0,0.0 +0,0.679700,0.0,0.0,0.0,0.0,0.0 +0,0.679800,0.0,0.0,0.0,0.0,0.0 +0,0.679900,0.0,0.0,0.0,0.0,0.0 +0,0.680000,0.0,0.0,0.0,0.0,0.0 +0,0.680100,0.0,0.0,0.0,0.0,0.0 +1,131.100052,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.680200,0.0,0.0,0.0,0.0,0.0 +0,0.680300,0.0,0.0,0.0,0.0,0.0 +0,0.680400,0.0,0.0,0.0,0.0,0.0 +0,0.680500,0.0,0.0,0.0,0.0,0.0 +0,0.680600,0.0,0.0,0.0,0.0,0.0 +0,0.680700,0.0,0.0,0.0,0.0,0.0 +0,0.680800,0.0,0.0,0.0,0.0,0.0 +0,0.680900,0.0,0.0,0.0,0.0,0.0 +0,0.681000,0.0,0.0,0.0,0.0,0.0 +0,0.681100,0.0,0.0,0.0,0.0,0.0 +0,0.681200,0.0,0.0,0.0,0.0,0.0 +0,0.681300,0.0,0.0,0.0,0.0,0.0 +0,0.681400,0.0,0.0,0.0,0.0,0.0 +0,0.681500,0.0,0.0,0.0,0.0,0.0 +0,0.681600,0.0,0.0,0.0,0.0,0.0 +0,0.681700,0.0,0.0,0.0,0.0,0.0 +0,0.681800,0.0,0.0,0.0,0.0,0.0 +0,0.681900,0.0,0.0,0.0,0.0,0.0 +0,0.682000,0.0,0.0,0.0,0.0,0.0 +0,0.682100,0.0,0.0,0.0,0.0,0.0 +0,0.682200,0.0,0.0,0.0,0.0,0.0 +0,0.682300,0.0,0.0,0.0,0.0,0.0 +0,0.682400,0.0,0.0,0.0,0.0,0.0 +0,0.682500,0.0,0.0,0.0,0.0,0.0 +0,0.682600,0.0,0.0,0.0,0.0,0.0 +0,0.682700,0.0,0.0,0.0,0.0,0.0 +0,0.682800,0.0,0.0,0.0,0.0,0.0 +0,0.682900,0.0,0.0,0.0,0.0,0.0 +0,0.683000,0.0,0.0,0.0,0.0,0.0 +0,0.683100,0.0,0.0,0.0,0.0,0.0 +0,0.683200,0.0,0.0,0.0,0.0,0.0 +0,0.683300,0.0,0.0,0.0,0.0,0.0 +0,0.683400,0.0,0.0,0.0,0.0,0.0 +0,0.683500,0.0,0.0,0.0,0.0,0.0 +0,0.683600,0.0,0.0,0.0,0.0,0.0 +0,0.683700,0.0,0.0,0.0,0.0,0.0 +0,0.683800,0.0,0.0,0.0,0.0,0.0 +0,0.683900,0.0,0.0,0.0,0.0,0.0 +0,0.684000,0.0,0.0,0.0,0.0,0.0 +0,0.684100,0.0,0.0,0.0,0.0,0.0 +0,0.684200,0.0,0.0,0.0,0.0,0.0 +0,0.684300,0.0,0.0,0.0,0.0,0.0 +0,0.684400,0.0,0.0,0.0,0.0,0.0 +0,0.684500,0.0,0.0,0.0,0.0,0.0 +0,0.684600,0.0,0.0,0.0,0.0,0.0 +0,0.684700,0.0,0.0,0.0,0.0,0.0 +0,0.684800,0.0,0.0,0.0,0.0,0.0 +0,0.684900,0.0,0.0,0.0,0.0,0.0 +0,0.685000,0.0,0.0,0.0,0.0,0.0 +0,0.685100,0.0,0.0,0.0,0.0,0.0 +0,0.685200,0.0,0.0,0.0,0.0,0.0 +0,0.685300,0.0,0.0,0.0,0.0,0.0 +0,0.685400,0.0,0.0,0.0,0.0,0.0 +0,0.685500,0.0,0.0,0.0,0.0,0.0 +0,0.685600,0.0,0.0,0.0,0.0,0.0 +0,0.685700,0.0,0.0,0.0,0.0,0.0 +0,0.685800,0.0,0.0,0.0,0.0,0.0 +0,0.685900,0.0,0.0,0.0,0.0,0.0 +0,0.686000,0.0,0.0,0.0,0.0,0.0 +0,0.686100,0.0,0.0,0.0,0.0,0.0 +0,0.686200,0.0,0.0,0.0,0.0,0.0 +0,0.686300,0.0,0.0,0.0,0.0,0.0 +0,0.686400,0.0,0.0,0.0,0.0,0.0 +0,0.686500,0.0,0.0,0.0,0.0,0.0 +0,0.686600,0.0,0.0,0.0,0.0,0.0 +0,0.686700,0.0,0.0,0.0,0.0,0.0 +0,0.686800,0.0,0.0,0.0,0.0,0.0 +0,0.686900,0.0,0.0,0.0,0.0,0.0 +0,0.687000,0.0,0.0,0.0,0.0,0.0 +0,0.687100,0.0,0.0,0.0,0.0,0.0 +0,0.687200,0.0,0.0,0.0,0.0,0.0 +0,0.687300,0.0,0.0,0.0,0.0,0.0 +0,0.687400,0.0,0.0,0.0,0.0,0.0 +0,0.687500,0.0,0.0,0.0,0.0,0.0 +0,0.687600,0.0,0.0,0.0,0.0,0.0 +0,0.687700,0.0,0.0,0.0,0.0,0.0 +0,0.687800,0.0,0.0,0.0,0.0,0.0 +0,0.687900,0.0,0.0,0.0,0.0,0.0 +0,0.688000,0.0,0.0,0.0,0.0,0.0 +0,0.688100,0.0,0.0,0.0,0.0,0.0 +0,0.688200,0.0,0.0,0.0,0.0,0.0 +0,0.688300,0.0,0.0,0.0,0.0,0.0 +0,0.688400,0.0,0.0,0.0,0.0,0.0 +0,0.688500,0.0,0.0,0.0,0.0,0.0 +0,0.688600,0.0,0.0,0.0,0.0,0.0 +0,0.688700,0.0,0.0,0.0,0.0,0.0 +0,0.688800,0.0,0.0,0.0,0.0,0.0 +0,0.688900,0.0,0.0,0.0,0.0,0.0 +0,0.689000,0.0,0.0,0.0,0.0,0.0 +0,0.689100,0.0,0.0,0.0,0.0,0.0 +0,0.689200,0.0,0.0,0.0,0.0,0.0 +0,0.689300,0.0,0.0,0.0,0.0,0.0 +0,0.689400,0.0,0.0,0.0,0.0,0.0 +0,0.689500,0.0,0.0,0.0,0.0,0.0 +0,0.689600,0.0,0.0,0.0,0.0,0.0 +0,0.689700,0.0,0.0,0.0,0.0,0.0 +0,0.689800,0.0,0.0,0.0,0.0,0.0 +0,0.689900,0.0,0.0,0.0,0.0,0.0 +0,0.690000,0.0,0.0,0.0,0.0,0.0 +0,0.690100,0.0,0.0,0.0,0.0,0.0 +0,0.690200,0.0,0.0,0.0,0.0,0.0 +0,0.690300,0.0,0.0,0.0,0.0,0.0 +0,0.690400,0.0,0.0,0.0,0.0,0.0 +0,0.690500,0.0,0.0,0.0,0.0,0.0 +0,0.690600,0.0,0.0,0.0,0.0,0.0 +0,0.690700,0.0,0.0,0.0,0.0,0.0 +0,0.690800,0.0,0.0,0.0,0.0,0.0 +0,0.690900,0.0,0.0,0.0,0.0,0.0 +0,0.691000,0.0,0.0,0.0,0.0,0.0 +0,0.691100,0.0,0.0,0.0,0.0,0.0 +0,0.691200,0.0,0.0,0.0,0.0,0.0 +0,0.691300,0.0,0.0,0.0,0.0,0.0 +0,0.691400,0.0,0.0,0.0,0.0,0.0 +0,0.691500,0.0,0.0,0.0,0.0,0.0 +0,0.691600,0.0,0.0,0.0,0.0,0.0 +0,0.691700,0.0,0.0,0.0,0.0,0.0 +0,0.691800,0.0,0.0,0.0,0.0,0.0 +0,0.691900,0.0,0.0,0.0,0.0,0.0 +0,0.692000,0.0,0.0,0.0,0.0,0.0 +0,0.692100,0.0,0.0,0.0,0.0,0.0 +0,0.692200,0.0,0.0,0.0,0.0,0.0 +0,0.692300,0.0,0.0,0.0,0.0,0.0 +0,0.692400,0.0,0.0,0.0,0.0,0.0 +0,0.692500,0.0,0.0,0.0,0.0,0.0 +0,0.692600,0.0,0.0,0.0,0.0,0.0 +0,0.692700,0.0,0.0,0.0,0.0,0.0 +0,0.692800,0.0,0.0,0.0,0.0,0.0 +0,0.692900,0.0,0.0,0.0,0.0,0.0 +0,0.693000,0.0,0.0,0.0,0.0,0.0 +0,0.693100,0.0,0.0,0.0,0.0,0.0 +0,0.693200,0.0,0.0,0.0,0.0,0.0 +0,0.693300,0.0,0.0,0.0,0.0,0.0 +0,0.693400,0.0,0.0,0.0,0.0,0.0 +0,0.693500,0.0,0.0,0.0,0.0,0.0 +0,0.693600,0.0,0.0,0.0,0.0,0.0 +0,0.693700,0.0,0.0,0.0,0.0,0.0 +0,0.693800,0.0,0.0,0.0,0.0,0.0 +0,0.693900,0.0,0.0,0.0,0.0,0.0 +0,0.694000,0.0,0.0,0.0,0.0,0.0 +0,0.694100,0.0,0.0,0.0,0.0,0.0 +0,0.694200,0.0,0.0,0.0,0.0,0.0 +0,0.694300,0.0,0.0,0.0,0.0,0.0 +0,0.694400,0.0,0.0,0.0,0.0,0.0 +0,0.694500,0.0,0.0,0.0,0.0,0.0 +0,0.694600,0.0,0.0,0.0,0.0,0.0 +0,0.694700,0.0,0.0,0.0,0.0,0.0 +0,0.694800,0.0,0.0,0.0,0.0,0.0 +0,0.694900,0.0,0.0,0.0,0.0,0.0 +0,0.695000,0.0,0.0,0.0,0.0,0.0 +0,0.695100,0.0,0.0,0.0,0.0,0.0 +0,0.695200,0.0,0.0,0.0,0.0,0.0 +0,0.695300,0.0,0.0,0.0,0.0,0.0 +0,0.695400,0.0,0.0,0.0,0.0,0.0 +0,0.695500,0.0,0.0,0.0,0.0,0.0 +0,0.695600,0.0,0.0,0.0,0.0,0.0 +0,0.695700,0.0,0.0,0.0,0.0,0.0 +0,0.695800,0.0,0.0,0.0,0.0,0.0 +0,0.695900,0.0,0.0,0.0,0.0,0.0 +0,0.696000,0.0,0.0,0.0,0.0,0.0 +0,0.696100,0.0,0.0,0.0,0.0,0.0 +0,0.696200,0.0,0.0,0.0,0.0,0.0 +0,0.696300,0.0,0.0,0.0,0.0,0.0 +0,0.696400,0.0,0.0,0.0,0.0,0.0 +0,0.696500,0.0,0.0,0.0,0.0,0.0 +0,0.696600,0.0,0.0,0.0,0.0,0.0 +0,0.696700,0.0,0.0,0.0,0.0,0.0 +0,0.696800,0.0,0.0,0.0,0.0,0.0 +0,0.696900,0.0,0.0,0.0,0.0,0.0 +0,0.697000,0.0,0.0,0.0,0.0,0.0 +0,0.697100,0.0,0.0,0.0,0.0,0.0 +0,0.697200,0.0,0.0,0.0,0.0,0.0 +0,0.697300,0.0,0.0,0.0,0.0,0.0 +0,0.697400,0.0,0.0,0.0,0.0,0.0 +0,0.697500,0.0,0.0,0.0,0.0,0.0 +0,0.697600,0.0,0.0,0.0,0.0,0.0 +0,0.697700,0.0,0.0,0.0,0.0,0.0 +0,0.697800,0.0,0.0,0.0,0.0,0.0 +0,0.697900,0.0,0.0,0.0,0.0,0.0 +0,0.698000,0.0,0.0,0.0,0.0,0.0 +0,0.698100,0.0,0.0,0.0,0.0,0.0 +0,0.698200,0.0,0.0,0.0,0.0,0.0 +0,0.698300,0.0,0.0,0.0,0.0,0.0 +0,0.698400,0.0,0.0,0.0,0.0,0.0 +0,0.698500,0.0,0.0,0.0,0.0,0.0 +0,0.698600,0.0,0.0,0.0,0.0,0.0 +0,0.698700,0.0,0.0,0.0,0.0,0.0 +0,0.698800,0.0,0.0,0.0,0.0,0.0 +0,0.698900,0.0,0.0,0.0,0.0,0.0 +0,0.699000,0.0,0.0,0.0,0.0,0.0 +0,0.699100,0.0,0.0,0.0,0.0,0.0 +0,0.699200,0.0,0.0,0.0,0.0,0.0 +0,0.699300,0.0,0.0,0.0,0.0,0.0 +0,0.699400,0.0,0.0,0.0,0.0,0.0 +0,0.699500,0.0,0.0,0.0,0.0,0.0 +0,0.699600,0.0,0.0,0.0,0.0,0.0 +0,0.699700,0.0,0.0,0.0,0.0,0.0 +0,0.699800,0.0,0.0,0.0,0.0,0.0 +0,0.699900,0.0,0.0,0.0,0.0,0.0 +0,0.700000,0.0,0.0,0.0,0.0,0.0 +0,0.700100,0.0,0.0,0.0,0.0,0.0 +1,143.008561,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.700200,0.0,0.0,0.0,0.0,0.0 +0,0.700300,0.0,0.0,0.0,0.0,0.0 +0,0.700400,0.0,0.0,0.0,0.0,0.0 +0,0.700500,0.0,0.0,0.0,0.0,0.0 +0,0.700600,0.0,0.0,0.0,0.0,0.0 +0,0.700700,0.0,0.0,0.0,0.0,0.0 +0,0.700800,0.0,0.0,0.0,0.0,0.0 +0,0.700900,0.0,0.0,0.0,0.0,0.0 +0,0.701000,0.0,0.0,0.0,0.0,0.0 +0,0.701100,0.0,0.0,0.0,0.0,0.0 +0,0.701200,0.0,0.0,0.0,0.0,0.0 +0,0.701300,0.0,0.0,0.0,0.0,0.0 +0,0.701400,0.0,0.0,0.0,0.0,0.0 +0,0.701500,0.0,0.0,0.0,0.0,0.0 +0,0.701600,0.0,0.0,0.0,0.0,0.0 +0,0.701700,0.0,0.0,0.0,0.0,0.0 +0,0.701800,0.0,0.0,0.0,0.0,0.0 +0,0.701900,0.0,0.0,0.0,0.0,0.0 +0,0.702000,0.0,0.0,0.0,0.0,0.0 +0,0.702100,0.0,0.0,0.0,0.0,0.0 +0,0.702200,0.0,0.0,0.0,0.0,0.0 +0,0.702300,0.0,0.0,0.0,0.0,0.0 +0,0.702400,0.0,0.0,0.0,0.0,0.0 +0,0.702500,0.0,0.0,0.0,0.0,0.0 +0,0.702600,0.0,0.0,0.0,0.0,0.0 +0,0.702700,0.0,0.0,0.0,0.0,0.0 +0,0.702800,0.0,0.0,0.0,0.0,0.0 +0,0.702900,0.0,0.0,0.0,0.0,0.0 +0,0.703000,0.0,0.0,0.0,0.0,0.0 +0,0.703100,0.0,0.0,0.0,0.0,0.0 +0,0.703200,0.0,0.0,0.0,0.0,0.0 +0,0.703300,0.0,0.0,0.0,0.0,0.0 +0,0.703400,0.0,0.0,0.0,0.0,0.0 +0,0.703500,0.0,0.0,0.0,0.0,0.0 +0,0.703600,0.0,0.0,0.0,0.0,0.0 +0,0.703700,0.0,0.0,0.0,0.0,0.0 +0,0.703800,0.0,0.0,0.0,0.0,0.0 +0,0.703900,0.0,0.0,0.0,0.0,0.0 +0,0.704000,0.0,0.0,0.0,0.0,0.0 +0,0.704100,0.0,0.0,0.0,0.0,0.0 +0,0.704200,0.0,0.0,0.0,0.0,0.0 +0,0.704300,0.0,0.0,0.0,0.0,0.0 +0,0.704400,0.0,0.0,0.0,0.0,0.0 +0,0.704500,0.0,0.0,0.0,0.0,0.0 +0,0.704600,0.0,0.0,0.0,0.0,0.0 +0,0.704700,0.0,0.0,0.0,0.0,0.0 +0,0.704800,0.0,0.0,0.0,0.0,0.0 +0,0.704900,0.0,0.0,0.0,0.0,0.0 +0,0.705000,0.0,0.0,0.0,0.0,0.0 +0,0.705100,0.0,0.0,0.0,0.0,0.0 +0,0.705200,0.0,0.0,0.0,0.0,0.0 +0,0.705300,0.0,0.0,0.0,0.0,0.0 +0,0.705400,0.0,0.0,0.0,0.0,0.0 +0,0.705500,0.0,0.0,0.0,0.0,0.0 +0,0.705600,0.0,0.0,0.0,0.0,0.0 +0,0.705700,0.0,0.0,0.0,0.0,0.0 +0,0.705800,0.0,0.0,0.0,0.0,0.0 +0,0.705900,0.0,0.0,0.0,0.0,0.0 +0,0.706000,0.0,0.0,0.0,0.0,0.0 +0,0.706100,0.0,0.0,0.0,0.0,0.0 +0,0.706200,0.0,0.0,0.0,0.0,0.0 +0,0.706300,0.0,0.0,0.0,0.0,0.0 +0,0.706400,0.0,0.0,0.0,0.0,0.0 +0,0.706500,0.0,0.0,0.0,0.0,0.0 +0,0.706600,0.0,0.0,0.0,0.0,0.0 +0,0.706700,0.0,0.0,0.0,0.0,0.0 +0,0.706800,0.0,0.0,0.0,0.0,0.0 +0,0.706900,0.0,0.0,0.0,0.0,0.0 +0,0.707000,0.0,0.0,0.0,0.0,0.0 +0,0.707100,0.0,0.0,0.0,0.0,0.0 +0,0.707200,0.0,0.0,0.0,0.0,0.0 +0,0.707300,0.0,0.0,0.0,0.0,0.0 +0,0.707400,0.0,0.0,0.0,0.0,0.0 +0,0.707500,0.0,0.0,0.0,0.0,0.0 +0,0.707600,0.0,0.0,0.0,0.0,0.0 +0,0.707700,0.0,0.0,0.0,0.0,0.0 +0,0.707800,0.0,0.0,0.0,0.0,0.0 +0,0.707900,0.0,0.0,0.0,0.0,0.0 +0,0.708000,0.0,0.0,0.0,0.0,0.0 +0,0.708100,0.0,0.0,0.0,0.0,0.0 +0,0.708200,0.0,0.0,0.0,0.0,0.0 +0,0.708300,0.0,0.0,0.0,0.0,0.0 +0,0.708400,0.0,0.0,0.0,0.0,0.0 +0,0.708500,0.0,0.0,0.0,0.0,0.0 +0,0.708600,0.0,0.0,0.0,0.0,0.0 +0,0.708700,0.0,0.0,0.0,0.0,0.0 +0,0.708800,0.0,0.0,0.0,0.0,0.0 +0,0.708900,0.0,0.0,0.0,0.0,0.0 +0,0.709000,0.0,0.0,0.0,0.0,0.0 +0,0.709100,0.0,0.0,0.0,0.0,0.0 +0,0.709200,0.0,0.0,0.0,0.0,0.0 +0,0.709300,0.0,0.0,0.0,0.0,0.0 +0,0.709400,0.0,0.0,0.0,0.0,0.0 +0,0.709500,0.0,0.0,0.0,0.0,0.0 +0,0.709600,0.0,0.0,0.0,0.0,0.0 +0,0.709700,0.0,0.0,0.0,0.0,0.0 +0,0.709800,0.0,0.0,0.0,0.0,0.0 +0,0.709900,0.0,0.0,0.0,0.0,0.0 +0,0.710000,0.0,0.0,0.0,0.0,0.0 +0,0.710100,0.0,0.0,0.0,0.0,0.0 +0,0.710200,0.0,0.0,0.0,0.0,0.0 +0,0.710300,0.0,0.0,0.0,0.0,0.0 +0,0.710400,0.0,0.0,0.0,0.0,0.0 +0,0.710500,0.0,0.0,0.0,0.0,0.0 +0,0.710600,0.0,0.0,0.0,0.0,0.0 +0,0.710700,0.0,0.0,0.0,0.0,0.0 +0,0.710800,0.0,0.0,0.0,0.0,0.0 +0,0.710900,0.0,0.0,0.0,0.0,0.0 +0,0.711000,0.0,0.0,0.0,0.0,0.0 +0,0.711100,0.0,0.0,0.0,0.0,0.0 +0,0.711200,0.0,0.0,0.0,0.0,0.0 +0,0.711300,0.0,0.0,0.0,0.0,0.0 +0,0.711400,0.0,0.0,0.0,0.0,0.0 +0,0.711500,0.0,0.0,0.0,0.0,0.0 +0,0.711600,0.0,0.0,0.0,0.0,0.0 +0,0.711700,0.0,0.0,0.0,0.0,0.0 +0,0.711800,0.0,0.0,0.0,0.0,0.0 +0,0.711900,0.0,0.0,0.0,0.0,0.0 +0,0.712000,0.0,0.0,0.0,0.0,0.0 +0,0.712100,0.0,0.0,0.0,0.0,0.0 +0,0.712200,0.0,0.0,0.0,0.0,0.0 +0,0.712300,0.0,0.0,0.0,0.0,0.0 +0,0.712400,0.0,0.0,0.0,0.0,0.0 +0,0.712500,0.0,0.0,0.0,0.0,0.0 +0,0.712600,0.0,0.0,0.0,0.0,0.0 +0,0.712700,0.0,0.0,0.0,0.0,0.0 +0,0.712800,0.0,0.0,0.0,0.0,0.0 +0,0.712900,0.0,0.0,0.0,0.0,0.0 +0,0.713000,0.0,0.0,0.0,0.0,0.0 +0,0.713100,0.0,0.0,0.0,0.0,0.0 +0,0.713200,0.0,0.0,0.0,0.0,0.0 +0,0.713300,0.0,0.0,0.0,0.0,0.0 +0,0.713400,0.0,0.0,0.0,0.0,0.0 +0,0.713500,0.0,0.0,0.0,0.0,0.0 +0,0.713600,0.0,0.0,0.0,0.0,0.0 +0,0.713700,0.0,0.0,0.0,0.0,0.0 +0,0.713800,0.0,0.0,0.0,0.0,0.0 +0,0.713900,0.0,0.0,0.0,0.0,0.0 +0,0.714000,0.0,0.0,0.0,0.0,0.0 +0,0.714100,0.0,0.0,0.0,0.0,0.0 +0,0.714200,0.0,0.0,0.0,0.0,0.0 +0,0.714300,0.0,0.0,0.0,0.0,0.0 +0,0.714400,0.0,0.0,0.0,0.0,0.0 +0,0.714500,0.0,0.0,0.0,0.0,0.0 +0,0.714600,0.0,0.0,0.0,0.0,0.0 +0,0.714700,0.0,0.0,0.0,0.0,0.0 +0,0.714800,0.0,0.0,0.0,0.0,0.0 +0,0.714900,0.0,0.0,0.0,0.0,0.0 +0,0.715000,0.0,0.0,0.0,0.0,0.0 +0,0.715100,0.0,0.0,0.0,0.0,0.0 +0,0.715200,0.0,0.0,0.0,0.0,0.0 +0,0.715300,0.0,0.0,0.0,0.0,0.0 +0,0.715400,0.0,0.0,0.0,0.0,0.0 +0,0.715500,0.0,0.0,0.0,0.0,0.0 +0,0.715600,0.0,0.0,0.0,0.0,0.0 +0,0.715700,0.0,0.0,0.0,0.0,0.0 +0,0.715800,0.0,0.0,0.0,0.0,0.0 +0,0.715900,0.0,0.0,0.0,0.0,0.0 +0,0.716000,0.0,0.0,0.0,0.0,0.0 +0,0.716100,0.0,0.0,0.0,0.0,0.0 +0,0.716200,0.0,0.0,0.0,0.0,0.0 +0,0.716300,0.0,0.0,0.0,0.0,0.0 +0,0.716400,0.0,0.0,0.0,0.0,0.0 +0,0.716500,0.0,0.0,0.0,0.0,0.0 +0,0.716600,0.0,0.0,0.0,0.0,0.0 +0,0.716700,0.0,0.0,0.0,0.0,0.0 +0,0.716800,0.0,0.0,0.0,0.0,0.0 +0,0.716900,0.0,0.0,0.0,0.0,0.0 +0,0.717000,0.0,0.0,0.0,0.0,0.0 +0,0.717100,0.0,0.0,0.0,0.0,0.0 +0,0.717200,0.0,0.0,0.0,0.0,0.0 +0,0.717300,0.0,0.0,0.0,0.0,0.0 +0,0.717400,0.0,0.0,0.0,0.0,0.0 +0,0.717500,0.0,0.0,0.0,0.0,0.0 +0,0.717600,0.0,0.0,0.0,0.0,0.0 +0,0.717700,0.0,0.0,0.0,0.0,0.0 +0,0.717800,0.0,0.0,0.0,0.0,0.0 +0,0.717900,0.0,0.0,0.0,0.0,0.0 +0,0.718000,0.0,0.0,0.0,0.0,0.0 +0,0.718100,0.0,0.0,0.0,0.0,0.0 +0,0.718200,0.0,0.0,0.0,0.0,0.0 +0,0.718300,0.0,0.0,0.0,0.0,0.0 +0,0.718400,0.0,0.0,0.0,0.0,0.0 +0,0.718500,0.0,0.0,0.0,0.0,0.0 +0,0.718600,0.0,0.0,0.0,0.0,0.0 +0,0.718700,0.0,0.0,0.0,0.0,0.0 +0,0.718800,0.0,0.0,0.0,0.0,0.0 +0,0.718900,0.0,0.0,0.0,0.0,0.0 +0,0.719000,0.0,0.0,0.0,0.0,0.0 +0,0.719100,0.0,0.0,0.0,0.0,0.0 +0,0.719200,0.0,0.0,0.0,0.0,0.0 +0,0.719300,0.0,0.0,0.0,0.0,0.0 +0,0.719400,0.0,0.0,0.0,0.0,0.0 +0,0.719500,0.0,0.0,0.0,0.0,0.0 +0,0.719600,0.0,0.0,0.0,0.0,0.0 +0,0.719700,0.0,0.0,0.0,0.0,0.0 +0,0.719800,0.0,0.0,0.0,0.0,0.0 +0,0.719900,0.0,0.0,0.0,0.0,0.0 +0,0.720000,0.0,0.0,0.0,0.0,0.0 +0,0.720100,0.0,0.0,0.0,0.0,0.0 +1,155.617220,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.720200,0.0,0.0,0.0,0.0,0.0 +0,0.720300,0.0,0.0,0.0,0.0,0.0 +0,0.720400,0.0,0.0,0.0,0.0,0.0 +0,0.720500,0.0,0.0,0.0,0.0,0.0 +0,0.720600,0.0,0.0,0.0,0.0,0.0 +0,0.720700,0.0,0.0,0.0,0.0,0.0 +0,0.720800,0.0,0.0,0.0,0.0,0.0 +0,0.720900,0.0,0.0,0.0,0.0,0.0 +0,0.721000,0.0,0.0,0.0,0.0,0.0 +0,0.721100,0.0,0.0,0.0,0.0,0.0 +0,0.721200,0.0,0.0,0.0,0.0,0.0 +0,0.721300,0.0,0.0,0.0,0.0,0.0 +0,0.721400,0.0,0.0,0.0,0.0,0.0 +0,0.721500,0.0,0.0,0.0,0.0,0.0 +0,0.721600,0.0,0.0,0.0,0.0,0.0 +0,0.721700,0.0,0.0,0.0,0.0,0.0 +0,0.721800,0.0,0.0,0.0,0.0,0.0 +0,0.721900,0.0,0.0,0.0,0.0,0.0 +0,0.722000,0.0,0.0,0.0,0.0,0.0 +0,0.722100,0.0,0.0,0.0,0.0,0.0 +0,0.722200,0.0,0.0,0.0,0.0,0.0 +0,0.722300,0.0,0.0,0.0,0.0,0.0 +0,0.722400,0.0,0.0,0.0,0.0,0.0 +0,0.722500,0.0,0.0,0.0,0.0,0.0 +0,0.722600,0.0,0.0,0.0,0.0,0.0 +0,0.722700,0.0,0.0,0.0,0.0,0.0 +0,0.722800,0.0,0.0,0.0,0.0,0.0 +0,0.722900,0.0,0.0,0.0,0.0,0.0 +0,0.723000,0.0,0.0,0.0,0.0,0.0 +0,0.723100,0.0,0.0,0.0,0.0,0.0 +0,0.723200,0.0,0.0,0.0,0.0,0.0 +0,0.723300,0.0,0.0,0.0,0.0,0.0 +0,0.723400,0.0,0.0,0.0,0.0,0.0 +0,0.723500,0.0,0.0,0.0,0.0,0.0 +0,0.723600,0.0,0.0,0.0,0.0,0.0 +0,0.723700,0.0,0.0,0.0,0.0,0.0 +0,0.723800,0.0,0.0,0.0,0.0,0.0 +0,0.723900,0.0,0.0,0.0,0.0,0.0 +0,0.724000,0.0,0.0,0.0,0.0,0.0 +0,0.724100,0.0,0.0,0.0,0.0,0.0 +0,0.724200,0.0,0.0,0.0,0.0,0.0 +0,0.724300,0.0,0.0,0.0,0.0,0.0 +0,0.724400,0.0,0.0,0.0,0.0,0.0 +0,0.724500,0.0,0.0,0.0,0.0,0.0 +0,0.724600,0.0,0.0,0.0,0.0,0.0 +0,0.724700,0.0,0.0,0.0,0.0,0.0 +0,0.724800,0.0,0.0,0.0,0.0,0.0 +0,0.724900,0.0,0.0,0.0,0.0,0.0 +0,0.725000,0.0,0.0,0.0,0.0,0.0 +0,0.725100,0.0,0.0,0.0,0.0,0.0 +0,0.725200,0.0,0.0,0.0,0.0,0.0 +0,0.725300,0.0,0.0,0.0,0.0,0.0 +0,0.725400,0.0,0.0,0.0,0.0,0.0 +0,0.725500,0.0,0.0,0.0,0.0,0.0 +0,0.725600,0.0,0.0,0.0,0.0,0.0 +0,0.725700,0.0,0.0,0.0,0.0,0.0 +0,0.725800,0.0,0.0,0.0,0.0,0.0 +0,0.725900,0.0,0.0,0.0,0.0,0.0 +0,0.726000,0.0,0.0,0.0,0.0,0.0 +0,0.726100,0.0,0.0,0.0,0.0,0.0 +0,0.726200,0.0,0.0,0.0,0.0,0.0 +0,0.726300,0.0,0.0,0.0,0.0,0.0 +0,0.726400,0.0,0.0,0.0,0.0,0.0 +0,0.726500,0.0,0.0,0.0,0.0,0.0 +0,0.726600,0.0,0.0,0.0,0.0,0.0 +0,0.726700,0.0,0.0,0.0,0.0,0.0 +0,0.726800,0.0,0.0,0.0,0.0,0.0 +0,0.726900,0.0,0.0,0.0,0.0,0.0 +0,0.727000,0.0,0.0,0.0,0.0,0.0 +0,0.727100,0.0,0.0,0.0,0.0,0.0 +0,0.727200,0.0,0.0,0.0,0.0,0.0 +0,0.727300,0.0,0.0,0.0,0.0,0.0 +0,0.727400,0.0,0.0,0.0,0.0,0.0 +0,0.727500,0.0,0.0,0.0,0.0,0.0 +0,0.727600,0.0,0.0,0.0,0.0,0.0 +0,0.727700,0.0,0.0,0.0,0.0,0.0 +0,0.727800,0.0,0.0,0.0,0.0,0.0 +0,0.727900,0.0,0.0,0.0,0.0,0.0 +0,0.728000,0.0,0.0,0.0,0.0,0.0 +0,0.728100,0.0,0.0,0.0,0.0,0.0 +0,0.728200,0.0,0.0,0.0,0.0,0.0 +0,0.728300,0.0,0.0,0.0,0.0,0.0 +0,0.728400,0.0,0.0,0.0,0.0,0.0 +0,0.728500,0.0,0.0,0.0,0.0,0.0 +0,0.728600,0.0,0.0,0.0,0.0,0.0 +0,0.728700,0.0,0.0,0.0,0.0,0.0 +0,0.728800,0.0,0.0,0.0,0.0,0.0 +0,0.728900,0.0,0.0,0.0,0.0,0.0 +0,0.729000,0.0,0.0,0.0,0.0,0.0 +0,0.729100,0.0,0.0,0.0,0.0,0.0 +0,0.729200,0.0,0.0,0.0,0.0,0.0 +0,0.729300,0.0,0.0,0.0,0.0,0.0 +0,0.729400,0.0,0.0,0.0,0.0,0.0 +0,0.729500,0.0,0.0,0.0,0.0,0.0 +0,0.729600,0.0,0.0,0.0,0.0,0.0 +0,0.729700,0.0,0.0,0.0,0.0,0.0 +0,0.729800,0.0,0.0,0.0,0.0,0.0 +0,0.729900,0.0,0.0,0.0,0.0,0.0 +0,0.730000,0.0,0.0,0.0,0.0,0.0 +0,0.730100,0.0,0.0,0.0,0.0,0.0 +0,0.730200,0.0,0.0,0.0,0.0,0.0 +0,0.730300,0.0,0.0,0.0,0.0,0.0 +0,0.730400,0.0,0.0,0.0,0.0,0.0 +0,0.730500,0.0,0.0,0.0,0.0,0.0 +0,0.730600,0.0,0.0,0.0,0.0,0.0 +0,0.730700,0.0,0.0,0.0,0.0,0.0 +0,0.730800,0.0,0.0,0.0,0.0,0.0 +0,0.730900,0.0,0.0,0.0,0.0,0.0 +0,0.731000,0.0,0.0,0.0,0.0,0.0 +0,0.731100,0.0,0.0,0.0,0.0,0.0 +0,0.731200,0.0,0.0,0.0,0.0,0.0 +0,0.731300,0.0,0.0,0.0,0.0,0.0 +0,0.731400,0.0,0.0,0.0,0.0,0.0 +0,0.731500,0.0,0.0,0.0,0.0,0.0 +0,0.731600,0.0,0.0,0.0,0.0,0.0 +0,0.731700,0.0,0.0,0.0,0.0,0.0 +0,0.731800,0.0,0.0,0.0,0.0,0.0 +0,0.731900,0.0,0.0,0.0,0.0,0.0 +0,0.732000,0.0,0.0,0.0,0.0,0.0 +0,0.732100,0.0,0.0,0.0,0.0,0.0 +0,0.732200,0.0,0.0,0.0,0.0,0.0 +0,0.732300,0.0,0.0,0.0,0.0,0.0 +0,0.732400,0.0,0.0,0.0,0.0,0.0 +0,0.732500,0.0,0.0,0.0,0.0,0.0 +0,0.732600,0.0,0.0,0.0,0.0,0.0 +0,0.732700,0.0,0.0,0.0,0.0,0.0 +0,0.732800,0.0,0.0,0.0,0.0,0.0 +0,0.732900,0.0,0.0,0.0,0.0,0.0 +0,0.733000,0.0,0.0,0.0,0.0,0.0 +0,0.733100,0.0,0.0,0.0,0.0,0.0 +0,0.733200,0.0,0.0,0.0,0.0,0.0 +0,0.733300,0.0,0.0,0.0,0.0,0.0 +0,0.733400,0.0,0.0,0.0,0.0,0.0 +0,0.733500,0.0,0.0,0.0,0.0,0.0 +0,0.733600,0.0,0.0,0.0,0.0,0.0 +0,0.733700,0.0,0.0,0.0,0.0,0.0 +0,0.733800,0.0,0.0,0.0,0.0,0.0 +0,0.733900,0.0,0.0,0.0,0.0,0.0 +0,0.734000,0.0,0.0,0.0,0.0,0.0 +0,0.734100,0.0,0.0,0.0,0.0,0.0 +0,0.734200,0.0,0.0,0.0,0.0,0.0 +0,0.734300,0.0,0.0,0.0,0.0,0.0 +0,0.734400,0.0,0.0,0.0,0.0,0.0 +0,0.734500,0.0,0.0,0.0,0.0,0.0 +0,0.734600,0.0,0.0,0.0,0.0,0.0 +0,0.734700,0.0,0.0,0.0,0.0,0.0 +0,0.734800,0.0,0.0,0.0,0.0,0.0 +0,0.734900,0.0,0.0,0.0,0.0,0.0 +0,0.735000,0.0,0.0,0.0,0.0,0.0 +0,0.735100,0.0,0.0,0.0,0.0,0.0 +0,0.735200,0.0,0.0,0.0,0.0,0.0 +0,0.735300,0.0,0.0,0.0,0.0,0.0 +0,0.735400,0.0,0.0,0.0,0.0,0.0 +0,0.735500,0.0,0.0,0.0,0.0,0.0 +0,0.735600,0.0,0.0,0.0,0.0,0.0 +0,0.735700,0.0,0.0,0.0,0.0,0.0 +0,0.735800,0.0,0.0,0.0,0.0,0.0 +0,0.735900,0.0,0.0,0.0,0.0,0.0 +0,0.736000,0.0,0.0,0.0,0.0,0.0 +0,0.736100,0.0,0.0,0.0,0.0,0.0 +0,0.736200,0.0,0.0,0.0,0.0,0.0 +0,0.736300,0.0,0.0,0.0,0.0,0.0 +0,0.736400,0.0,0.0,0.0,0.0,0.0 +0,0.736500,0.0,0.0,0.0,0.0,0.0 +0,0.736600,0.0,0.0,0.0,0.0,0.0 +0,0.736700,0.0,0.0,0.0,0.0,0.0 +0,0.736800,0.0,0.0,0.0,0.0,0.0 +0,0.736900,0.0,0.0,0.0,0.0,0.0 +0,0.737000,0.0,0.0,0.0,0.0,0.0 +0,0.737100,0.0,0.0,0.0,0.0,0.0 +0,0.737200,0.0,0.0,0.0,0.0,0.0 +0,0.737300,0.0,0.0,0.0,0.0,0.0 +0,0.737400,0.0,0.0,0.0,0.0,0.0 +0,0.737500,0.0,0.0,0.0,0.0,0.0 +0,0.737600,0.0,0.0,0.0,0.0,0.0 +0,0.737700,0.0,0.0,0.0,0.0,0.0 +0,0.737800,0.0,0.0,0.0,0.0,0.0 +0,0.737900,0.0,0.0,0.0,0.0,0.0 +0,0.738000,0.0,0.0,0.0,0.0,0.0 +0,0.738100,0.0,0.0,0.0,0.0,0.0 +0,0.738200,0.0,0.0,0.0,0.0,0.0 +0,0.738300,0.0,0.0,0.0,0.0,0.0 +0,0.738400,0.0,0.0,0.0,0.0,0.0 +0,0.738500,0.0,0.0,0.0,0.0,0.0 +0,0.738600,0.0,0.0,0.0,0.0,0.0 +0,0.738700,0.0,0.0,0.0,0.0,0.0 +0,0.738800,0.0,0.0,0.0,0.0,0.0 +0,0.738900,0.0,0.0,0.0,0.0,0.0 +0,0.739000,0.0,0.0,0.0,0.0,0.0 +0,0.739100,0.0,0.0,0.0,0.0,0.0 +0,0.739200,0.0,0.0,0.0,0.0,0.0 +0,0.739300,0.0,0.0,0.0,0.0,0.0 +0,0.739400,0.0,0.0,0.0,0.0,0.0 +0,0.739500,0.0,0.0,0.0,0.0,0.0 +0,0.739600,0.0,0.0,0.0,0.0,0.0 +0,0.739700,0.0,0.0,0.0,0.0,0.0 +0,0.739800,0.0,0.0,0.0,0.0,0.0 +0,0.739900,0.0,0.0,0.0,0.0,0.0 +0,0.740000,0.0,0.0,0.0,0.0,0.0 +0,0.740100,0.0,0.0,0.0,0.0,0.0 +1,168.946028,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.740200,0.0,0.0,0.0,0.0,0.0 +0,0.740300,0.0,0.0,0.0,0.0,0.0 +0,0.740400,0.0,0.0,0.0,0.0,0.0 +0,0.740500,0.0,0.0,0.0,0.0,0.0 +0,0.740600,0.0,0.0,0.0,0.0,0.0 +0,0.740700,0.0,0.0,0.0,0.0,0.0 +0,0.740800,0.0,0.0,0.0,0.0,0.0 +0,0.740900,0.0,0.0,0.0,0.0,0.0 +0,0.741000,0.0,0.0,0.0,0.0,0.0 +0,0.741100,0.0,0.0,0.0,0.0,0.0 +0,0.741200,0.0,0.0,0.0,0.0,0.0 +0,0.741300,0.0,0.0,0.0,0.0,0.0 +0,0.741400,0.0,0.0,0.0,0.0,0.0 +0,0.741500,0.0,0.0,0.0,0.0,0.0 +0,0.741600,0.0,0.0,0.0,0.0,0.0 +0,0.741700,0.0,0.0,0.0,0.0,0.0 +0,0.741800,0.0,0.0,0.0,0.0,0.0 +0,0.741900,0.0,0.0,0.0,0.0,0.0 +0,0.742000,0.0,0.0,0.0,0.0,0.0 +0,0.742100,0.0,0.0,0.0,0.0,0.0 +0,0.742200,0.0,0.0,0.0,0.0,0.0 +0,0.742300,0.0,0.0,0.0,0.0,0.0 +0,0.742400,0.0,0.0,0.0,0.0,0.0 +0,0.742500,0.0,0.0,0.0,0.0,0.0 +0,0.742600,0.0,0.0,0.0,0.0,0.0 +0,0.742700,0.0,0.0,0.0,0.0,0.0 +0,0.742800,0.0,0.0,0.0,0.0,0.0 +0,0.742900,0.0,0.0,0.0,0.0,0.0 +0,0.743000,0.0,0.0,0.0,0.0,0.0 +0,0.743100,0.0,0.0,0.0,0.0,0.0 +0,0.743200,0.0,0.0,0.0,0.0,0.0 +0,0.743300,0.0,0.0,0.0,0.0,0.0 +0,0.743400,0.0,0.0,0.0,0.0,0.0 +0,0.743500,0.0,0.0,0.0,0.0,0.0 +0,0.743600,0.0,0.0,0.0,0.0,0.0 +0,0.743700,0.0,0.0,0.0,0.0,0.0 +0,0.743800,0.0,0.0,0.0,0.0,0.0 +0,0.743900,0.0,0.0,0.0,0.0,0.0 +0,0.744000,0.0,0.0,0.0,0.0,0.0 +0,0.744100,0.0,0.0,0.0,0.0,0.0 +0,0.744200,0.0,0.0,0.0,0.0,0.0 +0,0.744300,0.0,0.0,0.0,0.0,0.0 +0,0.744400,0.0,0.0,0.0,0.0,0.0 +0,0.744500,0.0,0.0,0.0,0.0,0.0 +0,0.744600,0.0,0.0,0.0,0.0,0.0 +0,0.744700,0.0,0.0,0.0,0.0,0.0 +0,0.744800,0.0,0.0,0.0,0.0,0.0 +0,0.744900,0.0,0.0,0.0,0.0,0.0 +0,0.745000,0.0,0.0,0.0,0.0,0.0 +0,0.745100,0.0,0.0,0.0,0.0,0.0 +0,0.745200,0.0,0.0,0.0,0.0,0.0 +0,0.745300,0.0,0.0,0.0,0.0,0.0 +0,0.745400,0.0,0.0,0.0,0.0,0.0 +0,0.745500,0.0,0.0,0.0,0.0,0.0 +0,0.745600,0.0,0.0,0.0,0.0,0.0 +0,0.745700,0.0,0.0,0.0,0.0,0.0 +0,0.745800,0.0,0.0,0.0,0.0,0.0 +0,0.745900,0.0,0.0,0.0,0.0,0.0 +0,0.746000,0.0,0.0,0.0,0.0,0.0 +0,0.746100,0.0,0.0,0.0,0.0,0.0 +0,0.746200,0.0,0.0,0.0,0.0,0.0 +0,0.746300,0.0,0.0,0.0,0.0,0.0 +0,0.746400,0.0,0.0,0.0,0.0,0.0 +0,0.746500,0.0,0.0,0.0,0.0,0.0 +0,0.746600,0.0,0.0,0.0,0.0,0.0 +0,0.746700,0.0,0.0,0.0,0.0,0.0 +0,0.746800,0.0,0.0,0.0,0.0,0.0 +0,0.746900,0.0,0.0,0.0,0.0,0.0 +0,0.747000,0.0,0.0,0.0,0.0,0.0 +0,0.747100,0.0,0.0,0.0,0.0,0.0 +0,0.747200,0.0,0.0,0.0,0.0,0.0 +0,0.747300,0.0,0.0,0.0,0.0,0.0 +0,0.747400,0.0,0.0,0.0,0.0,0.0 +0,0.747500,0.0,0.0,0.0,0.0,0.0 +0,0.747600,0.0,0.0,0.0,0.0,0.0 +0,0.747700,0.0,0.0,0.0,0.0,0.0 +0,0.747800,0.0,0.0,0.0,0.0,0.0 +0,0.747900,0.0,0.0,0.0,0.0,0.0 +0,0.748000,0.0,0.0,0.0,0.0,0.0 +0,0.748100,0.0,0.0,0.0,0.0,0.0 +0,0.748200,0.0,0.0,0.0,0.0,0.0 +0,0.748300,0.0,0.0,0.0,0.0,0.0 +0,0.748400,0.0,0.0,0.0,0.0,0.0 +0,0.748500,0.0,0.0,0.0,0.0,0.0 +0,0.748600,0.0,0.0,0.0,0.0,0.0 +0,0.748700,0.0,0.0,0.0,0.0,0.0 +0,0.748800,0.0,0.0,0.0,0.0,0.0 +0,0.748900,0.0,0.0,0.0,0.0,0.0 +0,0.749000,0.0,0.0,0.0,0.0,0.0 +0,0.749100,0.0,0.0,0.0,0.0,0.0 +0,0.749200,0.0,0.0,0.0,0.0,0.0 +0,0.749300,0.0,0.0,0.0,0.0,0.0 +0,0.749400,0.0,0.0,0.0,0.0,0.0 +0,0.749500,0.0,0.0,0.0,0.0,0.0 +0,0.749600,0.0,0.0,0.0,0.0,0.0 +0,0.749700,0.0,0.0,0.0,0.0,0.0 +0,0.749800,0.0,0.0,0.0,0.0,0.0 +0,0.749900,0.0,0.0,0.0,0.0,0.0 +0,0.750000,0.0,0.0,0.0,0.0,0.0 +0,0.750100,0.0,0.0,0.0,0.0,0.0 +0,0.750200,0.0,0.0,0.0,0.0,0.0 +0,0.750300,0.0,0.0,0.0,0.0,0.0 +0,0.750400,0.0,0.0,0.0,0.0,0.0 +0,0.750500,0.0,0.0,0.0,0.0,0.0 +0,0.750600,0.0,0.0,0.0,0.0,0.0 +0,0.750700,0.0,0.0,0.0,0.0,0.0 +0,0.750800,0.0,0.0,0.0,0.0,0.0 +0,0.750900,0.0,0.0,0.0,0.0,0.0 +0,0.751000,0.0,0.0,0.0,0.0,0.0 +0,0.751100,0.0,0.0,0.0,0.0,0.0 +0,0.751200,0.0,0.0,0.0,0.0,0.0 +0,0.751300,0.0,0.0,0.0,0.0,0.0 +0,0.751400,0.0,0.0,0.0,0.0,0.0 +0,0.751500,0.0,0.0,0.0,0.0,0.0 +0,0.751600,0.0,0.0,0.0,0.0,0.0 +0,0.751700,0.0,0.0,0.0,0.0,0.0 +0,0.751800,0.0,0.0,0.0,0.0,0.0 +0,0.751900,0.0,0.0,0.0,0.0,0.0 +0,0.752000,0.0,0.0,0.0,0.0,0.0 +0,0.752100,0.0,0.0,0.0,0.0,0.0 +0,0.752200,0.0,0.0,0.0,0.0,0.0 +0,0.752300,0.0,0.0,0.0,0.0,0.0 +0,0.752400,0.0,0.0,0.0,0.0,0.0 +0,0.752500,0.0,0.0,0.0,0.0,0.0 +0,0.752600,0.0,0.0,0.0,0.0,0.0 +0,0.752700,0.0,0.0,0.0,0.0,0.0 +0,0.752800,0.0,0.0,0.0,0.0,0.0 +0,0.752900,0.0,0.0,0.0,0.0,0.0 +0,0.753000,0.0,0.0,0.0,0.0,0.0 +0,0.753100,0.0,0.0,0.0,0.0,0.0 +0,0.753200,0.0,0.0,0.0,0.0,0.0 +0,0.753300,0.0,0.0,0.0,0.0,0.0 +0,0.753400,0.0,0.0,0.0,0.0,0.0 +0,0.753500,0.0,0.0,0.0,0.0,0.0 +0,0.753600,0.0,0.0,0.0,0.0,0.0 +0,0.753700,0.0,0.0,0.0,0.0,0.0 +0,0.753800,0.0,0.0,0.0,0.0,0.0 +0,0.753900,0.0,0.0,0.0,0.0,0.0 +0,0.754000,0.0,0.0,0.0,0.0,0.0 +0,0.754100,0.0,0.0,0.0,0.0,0.0 +0,0.754200,0.0,0.0,0.0,0.0,0.0 +0,0.754300,0.0,0.0,0.0,0.0,0.0 +0,0.754400,0.0,0.0,0.0,0.0,0.0 +0,0.754500,0.0,0.0,0.0,0.0,0.0 +0,0.754600,0.0,0.0,0.0,0.0,0.0 +0,0.754700,0.0,0.0,0.0,0.0,0.0 +0,0.754800,0.0,0.0,0.0,0.0,0.0 +0,0.754900,0.0,0.0,0.0,0.0,0.0 +0,0.755000,0.0,0.0,0.0,0.0,0.0 +0,0.755100,0.0,0.0,0.0,0.0,0.0 +0,0.755200,0.0,0.0,0.0,0.0,0.0 +0,0.755300,0.0,0.0,0.0,0.0,0.0 +0,0.755400,0.0,0.0,0.0,0.0,0.0 +0,0.755500,0.0,0.0,0.0,0.0,0.0 +0,0.755600,0.0,0.0,0.0,0.0,0.0 +0,0.755700,0.0,0.0,0.0,0.0,0.0 +0,0.755800,0.0,0.0,0.0,0.0,0.0 +0,0.755900,0.0,0.0,0.0,0.0,0.0 +0,0.756000,0.0,0.0,0.0,0.0,0.0 +0,0.756100,0.0,0.0,0.0,0.0,0.0 +0,0.756200,0.0,0.0,0.0,0.0,0.0 +0,0.756300,0.0,0.0,0.0,0.0,0.0 +0,0.756400,0.0,0.0,0.0,0.0,0.0 +0,0.756500,0.0,0.0,0.0,0.0,0.0 +0,0.756600,0.0,0.0,0.0,0.0,0.0 +0,0.756700,0.0,0.0,0.0,0.0,0.0 +0,0.756800,0.0,0.0,0.0,0.0,0.0 +0,0.756900,0.0,0.0,0.0,0.0,0.0 +0,0.757000,0.0,0.0,0.0,0.0,0.0 +0,0.757100,0.0,0.0,0.0,0.0,0.0 +0,0.757200,0.0,0.0,0.0,0.0,0.0 +0,0.757300,0.0,0.0,0.0,0.0,0.0 +0,0.757400,0.0,0.0,0.0,0.0,0.0 +0,0.757500,0.0,0.0,0.0,0.0,0.0 +0,0.757600,0.0,0.0,0.0,0.0,0.0 +0,0.757700,0.0,0.0,0.0,0.0,0.0 +0,0.757800,0.0,0.0,0.0,0.0,0.0 +0,0.757900,0.0,0.0,0.0,0.0,0.0 +0,0.758000,0.0,0.0,0.0,0.0,0.0 +0,0.758100,0.0,0.0,0.0,0.0,0.0 +0,0.758200,0.0,0.0,0.0,0.0,0.0 +0,0.758300,0.0,0.0,0.0,0.0,0.0 +0,0.758400,0.0,0.0,0.0,0.0,0.0 +0,0.758500,0.0,0.0,0.0,0.0,0.0 +0,0.758600,0.0,0.0,0.0,0.0,0.0 +0,0.758700,0.0,0.0,0.0,0.0,0.0 +0,0.758800,0.0,0.0,0.0,0.0,0.0 +0,0.758900,0.0,0.0,0.0,0.0,0.0 +0,0.759000,0.0,0.0,0.0,0.0,0.0 +0,0.759100,0.0,0.0,0.0,0.0,0.0 +0,0.759200,0.0,0.0,0.0,0.0,0.0 +0,0.759300,0.0,0.0,0.0,0.0,0.0 +0,0.759400,0.0,0.0,0.0,0.0,0.0 +0,0.759500,0.0,0.0,0.0,0.0,0.0 +0,0.759600,0.0,0.0,0.0,0.0,0.0 +0,0.759700,0.0,0.0,0.0,0.0,0.0 +0,0.759800,0.0,0.0,0.0,0.0,0.0 +0,0.759900,0.0,0.0,0.0,0.0,0.0 +0,0.760000,0.0,0.0,0.0,0.0,0.0 +0,0.760100,0.0,0.0,0.0,0.0,0.0 +1,183.014987,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.760200,0.0,0.0,0.0,0.0,0.0 +0,0.760300,0.0,0.0,0.0,0.0,0.0 +0,0.760400,0.0,0.0,0.0,0.0,0.0 +0,0.760500,0.0,0.0,0.0,0.0,0.0 +0,0.760600,0.0,0.0,0.0,0.0,0.0 +0,0.760700,0.0,0.0,0.0,0.0,0.0 +0,0.760800,0.0,0.0,0.0,0.0,0.0 +0,0.760900,0.0,0.0,0.0,0.0,0.0 +0,0.761000,0.0,0.0,0.0,0.0,0.0 +0,0.761100,0.0,0.0,0.0,0.0,0.0 +0,0.761200,0.0,0.0,0.0,0.0,0.0 +0,0.761300,0.0,0.0,0.0,0.0,0.0 +0,0.761400,0.0,0.0,0.0,0.0,0.0 +0,0.761500,0.0,0.0,0.0,0.0,0.0 +0,0.761600,0.0,0.0,0.0,0.0,0.0 +0,0.761700,0.0,0.0,0.0,0.0,0.0 +0,0.761800,0.0,0.0,0.0,0.0,0.0 +0,0.761900,0.0,0.0,0.0,0.0,0.0 +0,0.762000,0.0,0.0,0.0,0.0,0.0 +0,0.762100,0.0,0.0,0.0,0.0,0.0 +0,0.762200,0.0,0.0,0.0,0.0,0.0 +0,0.762300,0.0,0.0,0.0,0.0,0.0 +0,0.762400,0.0,0.0,0.0,0.0,0.0 +0,0.762500,0.0,0.0,0.0,0.0,0.0 +0,0.762600,0.0,0.0,0.0,0.0,0.0 +0,0.762700,0.0,0.0,0.0,0.0,0.0 +0,0.762800,0.0,0.0,0.0,0.0,0.0 +0,0.762900,0.0,0.0,0.0,0.0,0.0 +0,0.763000,0.0,0.0,0.0,0.0,0.0 +0,0.763100,0.0,0.0,0.0,0.0,0.0 +0,0.763200,0.0,0.0,0.0,0.0,0.0 +0,0.763300,0.0,0.0,0.0,0.0,0.0 +0,0.763400,0.0,0.0,0.0,0.0,0.0 +0,0.763500,0.0,0.0,0.0,0.0,0.0 +0,0.763600,0.0,0.0,0.0,0.0,0.0 +0,0.763700,0.0,0.0,0.0,0.0,0.0 +0,0.763800,0.0,0.0,0.0,0.0,0.0 +0,0.763900,0.0,0.0,0.0,0.0,0.0 +0,0.764000,0.0,0.0,0.0,0.0,0.0 +0,0.764100,0.0,0.0,0.0,0.0,0.0 +0,0.764200,0.0,0.0,0.0,0.0,0.0 +0,0.764300,0.0,0.0,0.0,0.0,0.0 +0,0.764400,0.0,0.0,0.0,0.0,0.0 +0,0.764500,0.0,0.0,0.0,0.0,0.0 +0,0.764600,0.0,0.0,0.0,0.0,0.0 +0,0.764700,0.0,0.0,0.0,0.0,0.0 +0,0.764800,0.0,0.0,0.0,0.0,0.0 +0,0.764900,0.0,0.0,0.0,0.0,0.0 +0,0.765000,0.0,0.0,0.0,0.0,0.0 +0,0.765100,0.0,0.0,0.0,0.0,0.0 +0,0.765200,0.0,0.0,0.0,0.0,0.0 +0,0.765300,0.0,0.0,0.0,0.0,0.0 +0,0.765400,0.0,0.0,0.0,0.0,0.0 +0,0.765500,0.0,0.0,0.0,0.0,0.0 +0,0.765600,0.0,0.0,0.0,0.0,0.0 +0,0.765700,0.0,0.0,0.0,0.0,0.0 +0,0.765800,0.0,0.0,0.0,0.0,0.0 +0,0.765900,0.0,0.0,0.0,0.0,0.0 +0,0.766000,0.0,0.0,0.0,0.0,0.0 +0,0.766100,0.0,0.0,0.0,0.0,0.0 +0,0.766200,0.0,0.0,0.0,0.0,0.0 +0,0.766300,0.0,0.0,0.0,0.0,0.0 +0,0.766400,0.0,0.0,0.0,0.0,0.0 +0,0.766500,0.0,0.0,0.0,0.0,0.0 +0,0.766600,0.0,0.0,0.0,0.0,0.0 +0,0.766700,0.0,0.0,0.0,0.0,0.0 +0,0.766800,0.0,0.0,0.0,0.0,0.0 +0,0.766900,0.0,0.0,0.0,0.0,0.0 +0,0.767000,0.0,0.0,0.0,0.0,0.0 +0,0.767100,0.0,0.0,0.0,0.0,0.0 +0,0.767200,0.0,0.0,0.0,0.0,0.0 +0,0.767300,0.0,0.0,0.0,0.0,0.0 +0,0.767400,0.0,0.0,0.0,0.0,0.0 +0,0.767500,0.0,0.0,0.0,0.0,0.0 +0,0.767600,0.0,0.0,0.0,0.0,0.0 +0,0.767700,0.0,0.0,0.0,0.0,0.0 +0,0.767800,0.0,0.0,0.0,0.0,0.0 +0,0.767900,0.0,0.0,0.0,0.0,0.0 +0,0.768000,0.0,0.0,0.0,0.0,0.0 +0,0.768100,0.0,0.0,0.0,0.0,0.0 +0,0.768200,0.0,0.0,0.0,0.0,0.0 +0,0.768300,0.0,0.0,0.0,0.0,0.0 +0,0.768400,0.0,0.0,0.0,0.0,0.0 +0,0.768500,0.0,0.0,0.0,0.0,0.0 +0,0.768600,0.0,0.0,0.0,0.0,0.0 +0,0.768700,0.0,0.0,0.0,0.0,0.0 +0,0.768800,0.0,0.0,0.0,0.0,0.0 +0,0.768900,0.0,0.0,0.0,0.0,0.0 +0,0.769000,0.0,0.0,0.0,0.0,0.0 +0,0.769100,0.0,0.0,0.0,0.0,0.0 +0,0.769200,0.0,0.0,0.0,0.0,0.0 +0,0.769300,0.0,0.0,0.0,0.0,0.0 +0,0.769400,0.0,0.0,0.0,0.0,0.0 +0,0.769500,0.0,0.0,0.0,0.0,0.0 +0,0.769600,0.0,0.0,0.0,0.0,0.0 +0,0.769700,0.0,0.0,0.0,0.0,0.0 +0,0.769800,0.0,0.0,0.0,0.0,0.0 +0,0.769900,0.0,0.0,0.0,0.0,0.0 +0,0.770000,0.0,0.0,0.0,0.0,0.0 +0,0.770100,0.0,0.0,0.0,0.0,0.0 +0,0.770200,0.0,0.0,0.0,0.0,0.0 +0,0.770300,0.0,0.0,0.0,0.0,0.0 +0,0.770400,0.0,0.0,0.0,0.0,0.0 +0,0.770500,0.0,0.0,0.0,0.0,0.0 +0,0.770600,0.0,0.0,0.0,0.0,0.0 +0,0.770700,0.0,0.0,0.0,0.0,0.0 +0,0.770800,0.0,0.0,0.0,0.0,0.0 +0,0.770900,0.0,0.0,0.0,0.0,0.0 +0,0.771000,0.0,0.0,0.0,0.0,0.0 +0,0.771100,0.0,0.0,0.0,0.0,0.0 +0,0.771200,0.0,0.0,0.0,0.0,0.0 +0,0.771300,0.0,0.0,0.0,0.0,0.0 +0,0.771400,0.0,0.0,0.0,0.0,0.0 +0,0.771500,0.0,0.0,0.0,0.0,0.0 +0,0.771600,0.0,0.0,0.0,0.0,0.0 +0,0.771700,0.0,0.0,0.0,0.0,0.0 +0,0.771800,0.0,0.0,0.0,0.0,0.0 +0,0.771900,0.0,0.0,0.0,0.0,0.0 +0,0.772000,0.0,0.0,0.0,0.0,0.0 +0,0.772100,0.0,0.0,0.0,0.0,0.0 +0,0.772200,0.0,0.0,0.0,0.0,0.0 +0,0.772300,0.0,0.0,0.0,0.0,0.0 +0,0.772400,0.0,0.0,0.0,0.0,0.0 +0,0.772500,0.0,0.0,0.0,0.0,0.0 +0,0.772600,0.0,0.0,0.0,0.0,0.0 +0,0.772700,0.0,0.0,0.0,0.0,0.0 +0,0.772800,0.0,0.0,0.0,0.0,0.0 +0,0.772900,0.0,0.0,0.0,0.0,0.0 +0,0.773000,0.0,0.0,0.0,0.0,0.0 +0,0.773100,0.0,0.0,0.0,0.0,0.0 +0,0.773200,0.0,0.0,0.0,0.0,0.0 +0,0.773300,0.0,0.0,0.0,0.0,0.0 +0,0.773400,0.0,0.0,0.0,0.0,0.0 +0,0.773500,0.0,0.0,0.0,0.0,0.0 +0,0.773600,0.0,0.0,0.0,0.0,0.0 +0,0.773700,0.0,0.0,0.0,0.0,0.0 +0,0.773800,0.0,0.0,0.0,0.0,0.0 +0,0.773900,0.0,0.0,0.0,0.0,0.0 +0,0.774000,0.0,0.0,0.0,0.0,0.0 +0,0.774100,0.0,0.0,0.0,0.0,0.0 +0,0.774200,0.0,0.0,0.0,0.0,0.0 +0,0.774300,0.0,0.0,0.0,0.0,0.0 +0,0.774400,0.0,0.0,0.0,0.0,0.0 +0,0.774500,0.0,0.0,0.0,0.0,0.0 +0,0.774600,0.0,0.0,0.0,0.0,0.0 +0,0.774700,0.0,0.0,0.0,0.0,0.0 +0,0.774800,0.0,0.0,0.0,0.0,0.0 +0,0.774900,0.0,0.0,0.0,0.0,0.0 +0,0.775000,0.0,0.0,0.0,0.0,0.0 +0,0.775100,0.0,0.0,0.0,0.0,0.0 +0,0.775200,0.0,0.0,0.0,0.0,0.0 +0,0.775300,0.0,0.0,0.0,0.0,0.0 +0,0.775400,0.0,0.0,0.0,0.0,0.0 +0,0.775500,0.0,0.0,0.0,0.0,0.0 +0,0.775600,0.0,0.0,0.0,0.0,0.0 +0,0.775700,0.0,0.0,0.0,0.0,0.0 +0,0.775800,0.0,0.0,0.0,0.0,0.0 +0,0.775900,0.0,0.0,0.0,0.0,0.0 +0,0.776000,0.0,0.0,0.0,0.0,0.0 +0,0.776100,0.0,0.0,0.0,0.0,0.0 +0,0.776200,0.0,0.0,0.0,0.0,0.0 +0,0.776300,0.0,0.0,0.0,0.0,0.0 +0,0.776400,0.0,0.0,0.0,0.0,0.0 +0,0.776500,0.0,0.0,0.0,0.0,0.0 +0,0.776600,0.0,0.0,0.0,0.0,0.0 +0,0.776700,0.0,0.0,0.0,0.0,0.0 +0,0.776800,0.0,0.0,0.0,0.0,0.0 +0,0.776900,0.0,0.0,0.0,0.0,0.0 +0,0.777000,0.0,0.0,0.0,0.0,0.0 +0,0.777100,0.0,0.0,0.0,0.0,0.0 +0,0.777200,0.0,0.0,0.0,0.0,0.0 +0,0.777300,0.0,0.0,0.0,0.0,0.0 +0,0.777400,0.0,0.0,0.0,0.0,0.0 +0,0.777500,0.0,0.0,0.0,0.0,0.0 +0,0.777600,0.0,0.0,0.0,0.0,0.0 +0,0.777700,0.0,0.0,0.0,0.0,0.0 +0,0.777800,0.0,0.0,0.0,0.0,0.0 +0,0.777900,0.0,0.0,0.0,0.0,0.0 +0,0.778000,0.0,0.0,0.0,0.0,0.0 +0,0.778100,0.0,0.0,0.0,0.0,0.0 +0,0.778200,0.0,0.0,0.0,0.0,0.0 +0,0.778300,0.0,0.0,0.0,0.0,0.0 +0,0.778400,0.0,0.0,0.0,0.0,0.0 +0,0.778500,0.0,0.0,0.0,0.0,0.0 +0,0.778600,0.0,0.0,0.0,0.0,0.0 +0,0.778700,0.0,0.0,0.0,0.0,0.0 +0,0.778800,0.0,0.0,0.0,0.0,0.0 +0,0.778900,0.0,0.0,0.0,0.0,0.0 +0,0.779000,0.0,0.0,0.0,0.0,0.0 +0,0.779100,0.0,0.0,0.0,0.0,0.0 +0,0.779200,0.0,0.0,0.0,0.0,0.0 +0,0.779300,0.0,0.0,0.0,0.0,0.0 +0,0.779400,0.0,0.0,0.0,0.0,0.0 +0,0.779500,0.0,0.0,0.0,0.0,0.0 +0,0.779600,0.0,0.0,0.0,0.0,0.0 +0,0.779700,0.0,0.0,0.0,0.0,0.0 +0,0.779800,0.0,0.0,0.0,0.0,0.0 +0,0.779900,0.0,0.0,0.0,0.0,0.0 +0,0.780000,0.0,0.0,0.0,0.0,0.0 +0,0.780100,0.0,0.0,0.0,0.0,0.0 +1,197.844096,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.780200,0.0,0.0,0.0,0.0,0.0 +0,0.780300,0.0,0.0,0.0,0.0,0.0 +0,0.780400,0.0,0.0,0.0,0.0,0.0 +0,0.780500,0.0,0.0,0.0,0.0,0.0 +0,0.780600,0.0,0.0,0.0,0.0,0.0 +0,0.780700,0.0,0.0,0.0,0.0,0.0 +0,0.780800,0.0,0.0,0.0,0.0,0.0 +0,0.780900,0.0,0.0,0.0,0.0,0.0 +0,0.781000,0.0,0.0,0.0,0.0,0.0 +0,0.781100,0.0,0.0,0.0,0.0,0.0 +0,0.781200,0.0,0.0,0.0,0.0,0.0 +0,0.781300,0.0,0.0,0.0,0.0,0.0 +0,0.781400,0.0,0.0,0.0,0.0,0.0 +0,0.781500,0.0,0.0,0.0,0.0,0.0 +0,0.781600,0.0,0.0,0.0,0.0,0.0 +0,0.781700,0.0,0.0,0.0,0.0,0.0 +0,0.781800,0.0,0.0,0.0,0.0,0.0 +0,0.781900,0.0,0.0,0.0,0.0,0.0 +0,0.782000,0.0,0.0,0.0,0.0,0.0 +0,0.782100,0.0,0.0,0.0,0.0,0.0 +0,0.782200,0.0,0.0,0.0,0.0,0.0 +0,0.782300,0.0,0.0,0.0,0.0,0.0 +0,0.782400,0.0,0.0,0.0,0.0,0.0 +0,0.782500,0.0,0.0,0.0,0.0,0.0 +0,0.782600,0.0,0.0,0.0,0.0,0.0 +0,0.782700,0.0,0.0,0.0,0.0,0.0 +0,0.782800,0.0,0.0,0.0,0.0,0.0 +0,0.782900,0.0,0.0,0.0,0.0,0.0 +0,0.783000,0.0,0.0,0.0,0.0,0.0 +0,0.783100,0.0,0.0,0.0,0.0,0.0 +0,0.783200,0.0,0.0,0.0,0.0,0.0 +0,0.783300,0.0,0.0,0.0,0.0,0.0 +0,0.783400,0.0,0.0,0.0,0.0,0.0 +0,0.783500,0.0,0.0,0.0,0.0,0.0 +0,0.783600,0.0,0.0,0.0,0.0,0.0 +0,0.783700,0.0,0.0,0.0,0.0,0.0 +0,0.783800,0.0,0.0,0.0,0.0,0.0 +0,0.783900,0.0,0.0,0.0,0.0,0.0 +0,0.784000,0.0,0.0,0.0,0.0,0.0 +0,0.784100,0.0,0.0,0.0,0.0,0.0 +0,0.784200,0.0,0.0,0.0,0.0,0.0 +0,0.784300,0.0,0.0,0.0,0.0,0.0 +0,0.784400,0.0,0.0,0.0,0.0,0.0 +0,0.784500,0.0,0.0,0.0,0.0,0.0 +0,0.784600,0.0,0.0,0.0,0.0,0.0 +0,0.784700,0.0,0.0,0.0,0.0,0.0 +0,0.784800,0.0,0.0,0.0,0.0,0.0 +0,0.784900,0.0,0.0,0.0,0.0,0.0 +0,0.785000,0.0,0.0,0.0,0.0,0.0 +0,0.785100,0.0,0.0,0.0,0.0,0.0 +0,0.785200,0.0,0.0,0.0,0.0,0.0 +0,0.785300,0.0,0.0,0.0,0.0,0.0 +0,0.785400,0.0,0.0,0.0,0.0,0.0 +0,0.785500,0.0,0.0,0.0,0.0,0.0 +0,0.785600,0.0,0.0,0.0,0.0,0.0 +0,0.785700,0.0,0.0,0.0,0.0,0.0 +0,0.785800,0.0,0.0,0.0,0.0,0.0 +0,0.785900,0.0,0.0,0.0,0.0,0.0 +0,0.786000,0.0,0.0,0.0,0.0,0.0 +0,0.786100,0.0,0.0,0.0,0.0,0.0 +0,0.786200,0.0,0.0,0.0,0.0,0.0 +0,0.786300,0.0,0.0,0.0,0.0,0.0 +0,0.786400,0.0,0.0,0.0,0.0,0.0 +0,0.786500,0.0,0.0,0.0,0.0,0.0 +0,0.786600,0.0,0.0,0.0,0.0,0.0 +0,0.786700,0.0,0.0,0.0,0.0,0.0 +0,0.786800,0.0,0.0,0.0,0.0,0.0 +0,0.786900,0.0,0.0,0.0,0.0,0.0 +0,0.787000,0.0,0.0,0.0,0.0,0.0 +0,0.787100,0.0,0.0,0.0,0.0,0.0 +0,0.787200,0.0,0.0,0.0,0.0,0.0 +0,0.787300,0.0,0.0,0.0,0.0,0.0 +0,0.787400,0.0,0.0,0.0,0.0,0.0 +0,0.787500,0.0,0.0,0.0,0.0,0.0 +0,0.787600,0.0,0.0,0.0,0.0,0.0 +0,0.787700,0.0,0.0,0.0,0.0,0.0 +0,0.787800,0.0,0.0,0.0,0.0,0.0 +0,0.787900,0.0,0.0,0.0,0.0,0.0 +0,0.788000,0.0,0.0,0.0,0.0,0.0 +0,0.788100,0.0,0.0,0.0,0.0,0.0 +0,0.788200,0.0,0.0,0.0,0.0,0.0 +0,0.788300,0.0,0.0,0.0,0.0,0.0 +0,0.788400,0.0,0.0,0.0,0.0,0.0 +0,0.788500,0.0,0.0,0.0,0.0,0.0 +0,0.788600,0.0,0.0,0.0,0.0,0.0 +0,0.788700,0.0,0.0,0.0,0.0,0.0 +0,0.788800,0.0,0.0,0.0,0.0,0.0 +0,0.788900,0.0,0.0,0.0,0.0,0.0 +0,0.789000,0.0,0.0,0.0,0.0,0.0 +0,0.789100,0.0,0.0,0.0,0.0,0.0 +0,0.789200,0.0,0.0,0.0,0.0,0.0 +0,0.789300,0.0,0.0,0.0,0.0,0.0 +0,0.789400,0.0,0.0,0.0,0.0,0.0 +0,0.789500,0.0,0.0,0.0,0.0,0.0 +0,0.789600,0.0,0.0,0.0,0.0,0.0 +0,0.789700,0.0,0.0,0.0,0.0,0.0 +0,0.789800,0.0,0.0,0.0,0.0,0.0 +0,0.789900,0.0,0.0,0.0,0.0,0.0 +0,0.790000,0.0,0.0,0.0,0.0,0.0 +0,0.790100,0.0,0.0,0.0,0.0,0.0 +0,0.790200,0.0,0.0,0.0,0.0,0.0 +0,0.790300,0.0,0.0,0.0,0.0,0.0 +0,0.790400,0.0,0.0,0.0,0.0,0.0 +0,0.790500,0.0,0.0,0.0,0.0,0.0 +0,0.790600,0.0,0.0,0.0,0.0,0.0 +0,0.790700,0.0,0.0,0.0,0.0,0.0 +0,0.790800,0.0,0.0,0.0,0.0,0.0 +0,0.790900,0.0,0.0,0.0,0.0,0.0 +0,0.791000,0.0,0.0,0.0,0.0,0.0 +0,0.791100,0.0,0.0,0.0,0.0,0.0 +0,0.791200,0.0,0.0,0.0,0.0,0.0 +0,0.791300,0.0,0.0,0.0,0.0,0.0 +0,0.791400,0.0,0.0,0.0,0.0,0.0 +0,0.791500,0.0,0.0,0.0,0.0,0.0 +0,0.791600,0.0,0.0,0.0,0.0,0.0 +0,0.791700,0.0,0.0,0.0,0.0,0.0 +0,0.791800,0.0,0.0,0.0,0.0,0.0 +0,0.791900,0.0,0.0,0.0,0.0,0.0 +0,0.792000,0.0,0.0,0.0,0.0,0.0 +0,0.792100,0.0,0.0,0.0,0.0,0.0 +0,0.792200,0.0,0.0,0.0,0.0,0.0 +0,0.792300,0.0,0.0,0.0,0.0,0.0 +0,0.792400,0.0,0.0,0.0,0.0,0.0 +0,0.792500,0.0,0.0,0.0,0.0,0.0 +0,0.792600,0.0,0.0,0.0,0.0,0.0 +0,0.792700,0.0,0.0,0.0,0.0,0.0 +0,0.792800,0.0,0.0,0.0,0.0,0.0 +0,0.792900,0.0,0.0,0.0,0.0,0.0 +0,0.793000,0.0,0.0,0.0,0.0,0.0 +0,0.793100,0.0,0.0,0.0,0.0,0.0 +0,0.793200,0.0,0.0,0.0,0.0,0.0 +0,0.793300,0.0,0.0,0.0,0.0,0.0 +0,0.793400,0.0,0.0,0.0,0.0,0.0 +0,0.793500,0.0,0.0,0.0,0.0,0.0 +0,0.793600,0.0,0.0,0.0,0.0,0.0 +0,0.793700,0.0,0.0,0.0,0.0,0.0 +0,0.793800,0.0,0.0,0.0,0.0,0.0 +0,0.793900,0.0,0.0,0.0,0.0,0.0 +0,0.794000,0.0,0.0,0.0,0.0,0.0 +0,0.794100,0.0,0.0,0.0,0.0,0.0 +0,0.794200,0.0,0.0,0.0,0.0,0.0 +0,0.794300,0.0,0.0,0.0,0.0,0.0 +0,0.794400,0.0,0.0,0.0,0.0,0.0 +0,0.794500,0.0,0.0,0.0,0.0,0.0 +0,0.794600,0.0,0.0,0.0,0.0,0.0 +0,0.794700,0.0,0.0,0.0,0.0,0.0 +0,0.794800,0.0,0.0,0.0,0.0,0.0 +0,0.794900,0.0,0.0,0.0,0.0,0.0 +0,0.795000,0.0,0.0,0.0,0.0,0.0 +0,0.795100,0.0,0.0,0.0,0.0,0.0 +0,0.795200,0.0,0.0,0.0,0.0,0.0 +0,0.795300,0.0,0.0,0.0,0.0,0.0 +0,0.795400,0.0,0.0,0.0,0.0,0.0 +0,0.795500,0.0,0.0,0.0,0.0,0.0 +0,0.795600,0.0,0.0,0.0,0.0,0.0 +0,0.795700,0.0,0.0,0.0,0.0,0.0 +0,0.795800,0.0,0.0,0.0,0.0,0.0 +0,0.795900,0.0,0.0,0.0,0.0,0.0 +0,0.796000,0.0,0.0,0.0,0.0,0.0 +0,0.796100,0.0,0.0,0.0,0.0,0.0 +0,0.796200,0.0,0.0,0.0,0.0,0.0 +0,0.796300,0.0,0.0,0.0,0.0,0.0 +0,0.796400,0.0,0.0,0.0,0.0,0.0 +0,0.796500,0.0,0.0,0.0,0.0,0.0 +0,0.796600,0.0,0.0,0.0,0.0,0.0 +0,0.796700,0.0,0.0,0.0,0.0,0.0 +0,0.796800,0.0,0.0,0.0,0.0,0.0 +0,0.796900,0.0,0.0,0.0,0.0,0.0 +0,0.797000,0.0,0.0,0.0,0.0,0.0 +0,0.797100,0.0,0.0,0.0,0.0,0.0 +0,0.797200,0.0,0.0,0.0,0.0,0.0 +0,0.797300,0.0,0.0,0.0,0.0,0.0 +0,0.797400,0.0,0.0,0.0,0.0,0.0 +0,0.797500,0.0,0.0,0.0,0.0,0.0 +0,0.797600,0.0,0.0,0.0,0.0,0.0 +0,0.797700,0.0,0.0,0.0,0.0,0.0 +0,0.797800,0.0,0.0,0.0,0.0,0.0 +0,0.797900,0.0,0.0,0.0,0.0,0.0 +0,0.798000,0.0,0.0,0.0,0.0,0.0 +0,0.798100,0.0,0.0,0.0,0.0,0.0 +0,0.798200,0.0,0.0,0.0,0.0,0.0 +0,0.798300,0.0,0.0,0.0,0.0,0.0 +0,0.798400,0.0,0.0,0.0,0.0,0.0 +0,0.798500,0.0,0.0,0.0,0.0,0.0 +0,0.798600,0.0,0.0,0.0,0.0,0.0 +0,0.798700,0.0,0.0,0.0,0.0,0.0 +0,0.798800,0.0,0.0,0.0,0.0,0.0 +0,0.798900,0.0,0.0,0.0,0.0,0.0 +0,0.799000,0.0,0.0,0.0,0.0,0.0 +0,0.799100,0.0,0.0,0.0,0.0,0.0 +0,0.799200,0.0,0.0,0.0,0.0,0.0 +0,0.799300,0.0,0.0,0.0,0.0,0.0 +0,0.799400,0.0,0.0,0.0,0.0,0.0 +0,0.799500,0.0,0.0,0.0,0.0,0.0 +0,0.799600,0.0,0.0,0.0,0.0,0.0 +0,0.799700,0.0,0.0,0.0,0.0,0.0 +0,0.799800,0.0,0.0,0.0,0.0,0.0 +0,0.799900,0.0,0.0,0.0,0.0,0.0 +0,0.800000,0.0,0.0,0.0,0.0,0.0 +0,0.800100,0.0,0.0,0.0,0.0,0.0 +1,213.453355,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.800200,0.0,0.0,0.0,0.0,0.0 +0,0.800300,0.0,0.0,0.0,0.0,0.0 +0,0.800400,0.0,0.0,0.0,0.0,0.0 +0,0.800500,0.0,0.0,0.0,0.0,0.0 +0,0.800600,0.0,0.0,0.0,0.0,0.0 +0,0.800700,0.0,0.0,0.0,0.0,0.0 +0,0.800800,0.0,0.0,0.0,0.0,0.0 +0,0.800900,0.0,0.0,0.0,0.0,0.0 +0,0.801000,0.0,0.0,0.0,0.0,0.0 +0,0.801100,0.0,0.0,0.0,0.0,0.0 +0,0.801200,0.0,0.0,0.0,0.0,0.0 +0,0.801300,0.0,0.0,0.0,0.0,0.0 +0,0.801400,0.0,0.0,0.0,0.0,0.0 +0,0.801500,0.0,0.0,0.0,0.0,0.0 +0,0.801600,0.0,0.0,0.0,0.0,0.0 +0,0.801700,0.0,0.0,0.0,0.0,0.0 +0,0.801800,0.0,0.0,0.0,0.0,0.0 +0,0.801900,0.0,0.0,0.0,0.0,0.0 +0,0.802000,0.0,0.0,0.0,0.0,0.0 +0,0.802100,0.0,0.0,0.0,0.0,0.0 +0,0.802200,0.0,0.0,0.0,0.0,0.0 +0,0.802300,0.0,0.0,0.0,0.0,0.0 +0,0.802400,0.0,0.0,0.0,0.0,0.0 +0,0.802500,0.0,0.0,0.0,0.0,0.0 +0,0.802600,0.0,0.0,0.0,0.0,0.0 +0,0.802700,0.0,0.0,0.0,0.0,0.0 +0,0.802800,0.0,0.0,0.0,0.0,0.0 +0,0.802900,0.0,0.0,0.0,0.0,0.0 +0,0.803000,0.0,0.0,0.0,0.0,0.0 +0,0.803100,0.0,0.0,0.0,0.0,0.0 +0,0.803200,0.0,0.0,0.0,0.0,0.0 +0,0.803300,0.0,0.0,0.0,0.0,0.0 +0,0.803400,0.0,0.0,0.0,0.0,0.0 +0,0.803500,0.0,0.0,0.0,0.0,0.0 +0,0.803600,0.0,0.0,0.0,0.0,0.0 +0,0.803700,0.0,0.0,0.0,0.0,0.0 +0,0.803800,0.0,0.0,0.0,0.0,0.0 +0,0.803900,0.0,0.0,0.0,0.0,0.0 +0,0.804000,0.0,0.0,0.0,0.0,0.0 +0,0.804100,0.0,0.0,0.0,0.0,0.0 +0,0.804200,0.0,0.0,0.0,0.0,0.0 +0,0.804300,0.0,0.0,0.0,0.0,0.0 +0,0.804400,0.0,0.0,0.0,0.0,0.0 +0,0.804500,0.0,0.0,0.0,0.0,0.0 +0,0.804600,0.0,0.0,0.0,0.0,0.0 +0,0.804700,0.0,0.0,0.0,0.0,0.0 +0,0.804800,0.0,0.0,0.0,0.0,0.0 +0,0.804900,0.0,0.0,0.0,0.0,0.0 +0,0.805000,0.0,0.0,0.0,0.0,0.0 +0,0.805100,0.0,0.0,0.0,0.0,0.0 +0,0.805200,0.0,0.0,0.0,0.0,0.0 +0,0.805300,0.0,0.0,0.0,0.0,0.0 +0,0.805400,0.0,0.0,0.0,0.0,0.0 +0,0.805500,0.0,0.0,0.0,0.0,0.0 +0,0.805600,0.0,0.0,0.0,0.0,0.0 +0,0.805700,0.0,0.0,0.0,0.0,0.0 +0,0.805800,0.0,0.0,0.0,0.0,0.0 +0,0.805900,0.0,0.0,0.0,0.0,0.0 +0,0.806000,0.0,0.0,0.0,0.0,0.0 +0,0.806100,0.0,0.0,0.0,0.0,0.0 +0,0.806200,0.0,0.0,0.0,0.0,0.0 +0,0.806300,0.0,0.0,0.0,0.0,0.0 +0,0.806400,0.0,0.0,0.0,0.0,0.0 +0,0.806500,0.0,0.0,0.0,0.0,0.0 +0,0.806600,0.0,0.0,0.0,0.0,0.0 +0,0.806700,0.0,0.0,0.0,0.0,0.0 +0,0.806800,0.0,0.0,0.0,0.0,0.0 +0,0.806900,0.0,0.0,0.0,0.0,0.0 +0,0.807000,0.0,0.0,0.0,0.0,0.0 +0,0.807100,0.0,0.0,0.0,0.0,0.0 +0,0.807200,0.0,0.0,0.0,0.0,0.0 +0,0.807300,0.0,0.0,0.0,0.0,0.0 +0,0.807400,0.0,0.0,0.0,0.0,0.0 +0,0.807500,0.0,0.0,0.0,0.0,0.0 +0,0.807600,0.0,0.0,0.0,0.0,0.0 +0,0.807700,0.0,0.0,0.0,0.0,0.0 +0,0.807800,0.0,0.0,0.0,0.0,0.0 +0,0.807900,0.0,0.0,0.0,0.0,0.0 +0,0.808000,0.0,0.0,0.0,0.0,0.0 +0,0.808100,0.0,0.0,0.0,0.0,0.0 +0,0.808200,0.0,0.0,0.0,0.0,0.0 +0,0.808300,0.0,0.0,0.0,0.0,0.0 +0,0.808400,0.0,0.0,0.0,0.0,0.0 +0,0.808500,0.0,0.0,0.0,0.0,0.0 +0,0.808600,0.0,0.0,0.0,0.0,0.0 +0,0.808700,0.0,0.0,0.0,0.0,0.0 +0,0.808800,0.0,0.0,0.0,0.0,0.0 +0,0.808900,0.0,0.0,0.0,0.0,0.0 +0,0.809000,0.0,0.0,0.0,0.0,0.0 +0,0.809100,0.0,0.0,0.0,0.0,0.0 +0,0.809200,0.0,0.0,0.0,0.0,0.0 +0,0.809300,0.0,0.0,0.0,0.0,0.0 +0,0.809400,0.0,0.0,0.0,0.0,0.0 +0,0.809500,0.0,0.0,0.0,0.0,0.0 +0,0.809600,0.0,0.0,0.0,0.0,0.0 +0,0.809700,0.0,0.0,0.0,0.0,0.0 +0,0.809800,0.0,0.0,0.0,0.0,0.0 +0,0.809900,0.0,0.0,0.0,0.0,0.0 +0,0.810000,0.0,0.0,0.0,0.0,0.0 +0,0.810100,0.0,0.0,0.0,0.0,0.0 +0,0.810200,0.0,0.0,0.0,0.0,0.0 +0,0.810300,0.0,0.0,0.0,0.0,0.0 +0,0.810400,0.0,0.0,0.0,0.0,0.0 +0,0.810500,0.0,0.0,0.0,0.0,0.0 +0,0.810600,0.0,0.0,0.0,0.0,0.0 +0,0.810700,0.0,0.0,0.0,0.0,0.0 +0,0.810800,0.0,0.0,0.0,0.0,0.0 +0,0.810900,0.0,0.0,0.0,0.0,0.0 +0,0.811000,0.0,0.0,0.0,0.0,0.0 +0,0.811100,0.0,0.0,0.0,0.0,0.0 +0,0.811200,0.0,0.0,0.0,0.0,0.0 +0,0.811300,0.0,0.0,0.0,0.0,0.0 +0,0.811400,0.0,0.0,0.0,0.0,0.0 +0,0.811500,0.0,0.0,0.0,0.0,0.0 +0,0.811600,0.0,0.0,0.0,0.0,0.0 +0,0.811700,0.0,0.0,0.0,0.0,0.0 +0,0.811800,0.0,0.0,0.0,0.0,0.0 +0,0.811900,0.0,0.0,0.0,0.0,0.0 +0,0.812000,0.0,0.0,0.0,0.0,0.0 +0,0.812100,0.0,0.0,0.0,0.0,0.0 +0,0.812200,0.0,0.0,0.0,0.0,0.0 +0,0.812300,0.0,0.0,0.0,0.0,0.0 +0,0.812400,0.0,0.0,0.0,0.0,0.0 +0,0.812500,0.0,0.0,0.0,0.0,0.0 +0,0.812600,0.0,0.0,0.0,0.0,0.0 +0,0.812700,0.0,0.0,0.0,0.0,0.0 +0,0.812800,0.0,0.0,0.0,0.0,0.0 +0,0.812900,0.0,0.0,0.0,0.0,0.0 +0,0.813000,0.0,0.0,0.0,0.0,0.0 +0,0.813100,0.0,0.0,0.0,0.0,0.0 +0,0.813200,0.0,0.0,0.0,0.0,0.0 +0,0.813300,0.0,0.0,0.0,0.0,0.0 +0,0.813400,0.0,0.0,0.0,0.0,0.0 +0,0.813500,0.0,0.0,0.0,0.0,0.0 +0,0.813600,0.0,0.0,0.0,0.0,0.0 +0,0.813700,0.0,0.0,0.0,0.0,0.0 +0,0.813800,0.0,0.0,0.0,0.0,0.0 +0,0.813900,0.0,0.0,0.0,0.0,0.0 +0,0.814000,0.0,0.0,0.0,0.0,0.0 +0,0.814100,0.0,0.0,0.0,0.0,0.0 +0,0.814200,0.0,0.0,0.0,0.0,0.0 +0,0.814300,0.0,0.0,0.0,0.0,0.0 +0,0.814400,0.0,0.0,0.0,0.0,0.0 +0,0.814500,0.0,0.0,0.0,0.0,0.0 +0,0.814600,0.0,0.0,0.0,0.0,0.0 +0,0.814700,0.0,0.0,0.0,0.0,0.0 +0,0.814800,0.0,0.0,0.0,0.0,0.0 +0,0.814900,0.0,0.0,0.0,0.0,0.0 +0,0.815000,0.0,0.0,0.0,0.0,0.0 +0,0.815100,0.0,0.0,0.0,0.0,0.0 +0,0.815200,0.0,0.0,0.0,0.0,0.0 +0,0.815300,0.0,0.0,0.0,0.0,0.0 +0,0.815400,0.0,0.0,0.0,0.0,0.0 +0,0.815500,0.0,0.0,0.0,0.0,0.0 +0,0.815600,0.0,0.0,0.0,0.0,0.0 +0,0.815700,0.0,0.0,0.0,0.0,0.0 +0,0.815800,0.0,0.0,0.0,0.0,0.0 +0,0.815900,0.0,0.0,0.0,0.0,0.0 +0,0.816000,0.0,0.0,0.0,0.0,0.0 +0,0.816100,0.0,0.0,0.0,0.0,0.0 +0,0.816200,0.0,0.0,0.0,0.0,0.0 +0,0.816300,0.0,0.0,0.0,0.0,0.0 +0,0.816400,0.0,0.0,0.0,0.0,0.0 +0,0.816500,0.0,0.0,0.0,0.0,0.0 +0,0.816600,0.0,0.0,0.0,0.0,0.0 +0,0.816700,0.0,0.0,0.0,0.0,0.0 +0,0.816800,0.0,0.0,0.0,0.0,0.0 +0,0.816900,0.0,0.0,0.0,0.0,0.0 +0,0.817000,0.0,0.0,0.0,0.0,0.0 +0,0.817100,0.0,0.0,0.0,0.0,0.0 +0,0.817200,0.0,0.0,0.0,0.0,0.0 +0,0.817300,0.0,0.0,0.0,0.0,0.0 +0,0.817400,0.0,0.0,0.0,0.0,0.0 +0,0.817500,0.0,0.0,0.0,0.0,0.0 +0,0.817600,0.0,0.0,0.0,0.0,0.0 +0,0.817700,0.0,0.0,0.0,0.0,0.0 +0,0.817800,0.0,0.0,0.0,0.0,0.0 +0,0.817900,0.0,0.0,0.0,0.0,0.0 +0,0.818000,0.0,0.0,0.0,0.0,0.0 +0,0.818100,0.0,0.0,0.0,0.0,0.0 +0,0.818200,0.0,0.0,0.0,0.0,0.0 +0,0.818300,0.0,0.0,0.0,0.0,0.0 +0,0.818400,0.0,0.0,0.0,0.0,0.0 +0,0.818500,0.0,0.0,0.0,0.0,0.0 +0,0.818600,0.0,0.0,0.0,0.0,0.0 +0,0.818700,0.0,0.0,0.0,0.0,0.0 +0,0.818800,0.0,0.0,0.0,0.0,0.0 +0,0.818900,0.0,0.0,0.0,0.0,0.0 +0,0.819000,0.0,0.0,0.0,0.0,0.0 +0,0.819100,0.0,0.0,0.0,0.0,0.0 +0,0.819200,0.0,0.0,0.0,0.0,0.0 +0,0.819300,0.0,0.0,0.0,0.0,0.0 +0,0.819400,0.0,0.0,0.0,0.0,0.0 +0,0.819500,0.0,0.0,0.0,0.0,0.0 +0,0.819600,0.0,0.0,0.0,0.0,0.0 +0,0.819700,0.0,0.0,0.0,0.0,0.0 +0,0.819800,0.0,0.0,0.0,0.0,0.0 +0,0.819900,0.0,0.0,0.0,0.0,0.0 +0,0.820000,0.0,0.0,0.0,0.0,0.0 +0,0.820100,0.0,0.0,0.0,0.0,0.0 +1,229.862764,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.820200,0.0,0.0,0.0,0.0,0.0 +0,0.820300,0.0,0.0,0.0,0.0,0.0 +0,0.820400,0.0,0.0,0.0,0.0,0.0 +0,0.820500,0.0,0.0,0.0,0.0,0.0 +0,0.820600,0.0,0.0,0.0,0.0,0.0 +0,0.820700,0.0,0.0,0.0,0.0,0.0 +0,0.820800,0.0,0.0,0.0,0.0,0.0 +0,0.820900,0.0,0.0,0.0,0.0,0.0 +0,0.821000,0.0,0.0,0.0,0.0,0.0 +0,0.821100,0.0,0.0,0.0,0.0,0.0 +0,0.821200,0.0,0.0,0.0,0.0,0.0 +0,0.821300,0.0,0.0,0.0,0.0,0.0 +0,0.821400,0.0,0.0,0.0,0.0,0.0 +0,0.821500,0.0,0.0,0.0,0.0,0.0 +0,0.821600,0.0,0.0,0.0,0.0,0.0 +0,0.821700,0.0,0.0,0.0,0.0,0.0 +0,0.821800,0.0,0.0,0.0,0.0,0.0 +0,0.821900,0.0,0.0,0.0,0.0,0.0 +0,0.822000,0.0,0.0,0.0,0.0,0.0 +0,0.822100,0.0,0.0,0.0,0.0,0.0 +0,0.822200,0.0,0.0,0.0,0.0,0.0 +0,0.822300,0.0,0.0,0.0,0.0,0.0 +0,0.822400,0.0,0.0,0.0,0.0,0.0 +0,0.822500,0.0,0.0,0.0,0.0,0.0 +0,0.822600,0.0,0.0,0.0,0.0,0.0 +0,0.822700,0.0,0.0,0.0,0.0,0.0 +0,0.822800,0.0,0.0,0.0,0.0,0.0 +0,0.822900,0.0,0.0,0.0,0.0,0.0 +0,0.823000,0.0,0.0,0.0,0.0,0.0 +0,0.823100,0.0,0.0,0.0,0.0,0.0 +0,0.823200,0.0,0.0,0.0,0.0,0.0 +0,0.823300,0.0,0.0,0.0,0.0,0.0 +0,0.823400,0.0,0.0,0.0,0.0,0.0 +0,0.823500,0.0,0.0,0.0,0.0,0.0 +0,0.823600,0.0,0.0,0.0,0.0,0.0 +0,0.823700,0.0,0.0,0.0,0.0,0.0 +0,0.823800,0.0,0.0,0.0,0.0,0.0 +0,0.823900,0.0,0.0,0.0,0.0,0.0 +0,0.824000,0.0,0.0,0.0,0.0,0.0 +0,0.824100,0.0,0.0,0.0,0.0,0.0 +0,0.824200,0.0,0.0,0.0,0.0,0.0 +0,0.824300,0.0,0.0,0.0,0.0,0.0 +0,0.824400,0.0,0.0,0.0,0.0,0.0 +0,0.824500,0.0,0.0,0.0,0.0,0.0 +0,0.824600,0.0,0.0,0.0,0.0,0.0 +0,0.824700,0.0,0.0,0.0,0.0,0.0 +0,0.824800,0.0,0.0,0.0,0.0,0.0 +0,0.824900,0.0,0.0,0.0,0.0,0.0 +0,0.825000,0.0,0.0,0.0,0.0,0.0 +0,0.825100,0.0,0.0,0.0,0.0,0.0 +0,0.825200,0.0,0.0,0.0,0.0,0.0 +0,0.825300,0.0,0.0,0.0,0.0,0.0 +0,0.825400,0.0,0.0,0.0,0.0,0.0 +0,0.825500,0.0,0.0,0.0,0.0,0.0 +0,0.825600,0.0,0.0,0.0,0.0,0.0 +0,0.825700,0.0,0.0,0.0,0.0,0.0 +0,0.825800,0.0,0.0,0.0,0.0,0.0 +0,0.825900,0.0,0.0,0.0,0.0,0.0 +0,0.826000,0.0,0.0,0.0,0.0,0.0 +0,0.826100,0.0,0.0,0.0,0.0,0.0 +0,0.826200,0.0,0.0,0.0,0.0,0.0 +0,0.826300,0.0,0.0,0.0,0.0,0.0 +0,0.826400,0.0,0.0,0.0,0.0,0.0 +0,0.826500,0.0,0.0,0.0,0.0,0.0 +0,0.826600,0.0,0.0,0.0,0.0,0.0 +0,0.826700,0.0,0.0,0.0,0.0,0.0 +0,0.826800,0.0,0.0,0.0,0.0,0.0 +0,0.826900,0.0,0.0,0.0,0.0,0.0 +0,0.827000,0.0,0.0,0.0,0.0,0.0 +0,0.827100,0.0,0.0,0.0,0.0,0.0 +0,0.827200,0.0,0.0,0.0,0.0,0.0 +0,0.827300,0.0,0.0,0.0,0.0,0.0 +0,0.827400,0.0,0.0,0.0,0.0,0.0 +0,0.827500,0.0,0.0,0.0,0.0,0.0 +0,0.827600,0.0,0.0,0.0,0.0,0.0 +0,0.827700,0.0,0.0,0.0,0.0,0.0 +0,0.827800,0.0,0.0,0.0,0.0,0.0 +0,0.827900,0.0,0.0,0.0,0.0,0.0 +0,0.828000,0.0,0.0,0.0,0.0,0.0 +0,0.828100,0.0,0.0,0.0,0.0,0.0 +0,0.828200,0.0,0.0,0.0,0.0,0.0 +0,0.828300,0.0,0.0,0.0,0.0,0.0 +0,0.828400,0.0,0.0,0.0,0.0,0.0 +0,0.828500,0.0,0.0,0.0,0.0,0.0 +0,0.828600,0.0,0.0,0.0,0.0,0.0 +0,0.828700,0.0,0.0,0.0,0.0,0.0 +0,0.828800,0.0,0.0,0.0,0.0,0.0 +0,0.828900,0.0,0.0,0.0,0.0,0.0 +0,0.829000,0.0,0.0,0.0,0.0,0.0 +0,0.829100,0.0,0.0,0.0,0.0,0.0 +0,0.829200,0.0,0.0,0.0,0.0,0.0 +0,0.829300,0.0,0.0,0.0,0.0,0.0 +0,0.829400,0.0,0.0,0.0,0.0,0.0 +0,0.829500,0.0,0.0,0.0,0.0,0.0 +0,0.829600,0.0,0.0,0.0,0.0,0.0 +0,0.829700,0.0,0.0,0.0,0.0,0.0 +0,0.829800,0.0,0.0,0.0,0.0,0.0 +0,0.829900,0.0,0.0,0.0,0.0,0.0 +0,0.830000,0.0,0.0,0.0,0.0,0.0 +0,0.830100,0.0,0.0,0.0,0.0,0.0 +0,0.830200,0.0,0.0,0.0,0.0,0.0 +0,0.830300,0.0,0.0,0.0,0.0,0.0 +0,0.830400,0.0,0.0,0.0,0.0,0.0 +0,0.830500,0.0,0.0,0.0,0.0,0.0 +0,0.830600,0.0,0.0,0.0,0.0,0.0 +0,0.830700,0.0,0.0,0.0,0.0,0.0 +0,0.830800,0.0,0.0,0.0,0.0,0.0 +0,0.830900,0.0,0.0,0.0,0.0,0.0 +0,0.831000,0.0,0.0,0.0,0.0,0.0 +0,0.831100,0.0,0.0,0.0,0.0,0.0 +0,0.831200,0.0,0.0,0.0,0.0,0.0 +0,0.831300,0.0,0.0,0.0,0.0,0.0 +0,0.831400,0.0,0.0,0.0,0.0,0.0 +0,0.831500,0.0,0.0,0.0,0.0,0.0 +0,0.831600,0.0,0.0,0.0,0.0,0.0 +0,0.831700,0.0,0.0,0.0,0.0,0.0 +0,0.831800,0.0,0.0,0.0,0.0,0.0 +0,0.831900,0.0,0.0,0.0,0.0,0.0 +0,0.832000,0.0,0.0,0.0,0.0,0.0 +0,0.832100,0.0,0.0,0.0,0.0,0.0 +0,0.832200,0.0,0.0,0.0,0.0,0.0 +0,0.832300,0.0,0.0,0.0,0.0,0.0 +0,0.832400,0.0,0.0,0.0,0.0,0.0 +0,0.832500,0.0,0.0,0.0,0.0,0.0 +0,0.832600,0.0,0.0,0.0,0.0,0.0 +0,0.832700,0.0,0.0,0.0,0.0,0.0 +0,0.832800,0.0,0.0,0.0,0.0,0.0 +0,0.832900,0.0,0.0,0.0,0.0,0.0 +0,0.833000,0.0,0.0,0.0,0.0,0.0 +0,0.833100,0.0,0.0,0.0,0.0,0.0 +0,0.833200,0.0,0.0,0.0,0.0,0.0 +0,0.833300,0.0,0.0,0.0,0.0,0.0 +0,0.833400,0.0,0.0,0.0,0.0,0.0 +0,0.833500,0.0,0.0,0.0,0.0,0.0 +0,0.833600,0.0,0.0,0.0,0.0,0.0 +0,0.833700,0.0,0.0,0.0,0.0,0.0 +0,0.833800,0.0,0.0,0.0,0.0,0.0 +0,0.833900,0.0,0.0,0.0,0.0,0.0 +0,0.834000,0.0,0.0,0.0,0.0,0.0 +0,0.834100,0.0,0.0,0.0,0.0,0.0 +0,0.834200,0.0,0.0,0.0,0.0,0.0 +0,0.834300,0.0,0.0,0.0,0.0,0.0 +0,0.834400,0.0,0.0,0.0,0.0,0.0 +0,0.834500,0.0,0.0,0.0,0.0,0.0 +0,0.834600,0.0,0.0,0.0,0.0,0.0 +0,0.834700,0.0,0.0,0.0,0.0,0.0 +0,0.834800,0.0,0.0,0.0,0.0,0.0 +0,0.834900,0.0,0.0,0.0,0.0,0.0 +0,0.835000,0.0,0.0,0.0,0.0,0.0 +0,0.835100,0.0,0.0,0.0,0.0,0.0 +0,0.835200,0.0,0.0,0.0,0.0,0.0 +0,0.835300,0.0,0.0,0.0,0.0,0.0 +0,0.835400,0.0,0.0,0.0,0.0,0.0 +0,0.835500,0.0,0.0,0.0,0.0,0.0 +0,0.835600,0.0,0.0,0.0,0.0,0.0 +0,0.835700,0.0,0.0,0.0,0.0,0.0 +0,0.835800,0.0,0.0,0.0,0.0,0.0 +0,0.835900,0.0,0.0,0.0,0.0,0.0 +0,0.836000,0.0,0.0,0.0,0.0,0.0 +0,0.836100,0.0,0.0,0.0,0.0,0.0 +0,0.836200,0.0,0.0,0.0,0.0,0.0 +0,0.836300,0.0,0.0,0.0,0.0,0.0 +0,0.836400,0.0,0.0,0.0,0.0,0.0 +0,0.836500,0.0,0.0,0.0,0.0,0.0 +0,0.836600,0.0,0.0,0.0,0.0,0.0 +0,0.836700,0.0,0.0,0.0,0.0,0.0 +0,0.836800,0.0,0.0,0.0,0.0,0.0 +0,0.836900,0.0,0.0,0.0,0.0,0.0 +0,0.837000,0.0,0.0,0.0,0.0,0.0 +0,0.837100,0.0,0.0,0.0,0.0,0.0 +0,0.837200,0.0,0.0,0.0,0.0,0.0 +0,0.837300,0.0,0.0,0.0,0.0,0.0 +0,0.837400,0.0,0.0,0.0,0.0,0.0 +0,0.837500,0.0,0.0,0.0,0.0,0.0 +0,0.837600,0.0,0.0,0.0,0.0,0.0 +0,0.837700,0.0,0.0,0.0,0.0,0.0 +0,0.837800,0.0,0.0,0.0,0.0,0.0 +0,0.837900,0.0,0.0,0.0,0.0,0.0 +0,0.838000,0.0,0.0,0.0,0.0,0.0 +0,0.838100,0.0,0.0,0.0,0.0,0.0 +0,0.838200,0.0,0.0,0.0,0.0,0.0 +0,0.838300,0.0,0.0,0.0,0.0,0.0 +0,0.838400,0.0,0.0,0.0,0.0,0.0 +0,0.838500,0.0,0.0,0.0,0.0,0.0 +0,0.838600,0.0,0.0,0.0,0.0,0.0 +0,0.838700,0.0,0.0,0.0,0.0,0.0 +0,0.838800,0.0,0.0,0.0,0.0,0.0 +0,0.838900,0.0,0.0,0.0,0.0,0.0 +0,0.839000,0.0,0.0,0.0,0.0,0.0 +0,0.839100,0.0,0.0,0.0,0.0,0.0 +0,0.839200,0.0,0.0,0.0,0.0,0.0 +0,0.839300,0.0,0.0,0.0,0.0,0.0 +0,0.839400,0.0,0.0,0.0,0.0,0.0 +0,0.839500,0.0,0.0,0.0,0.0,0.0 +0,0.839600,0.0,0.0,0.0,0.0,0.0 +0,0.839700,0.0,0.0,0.0,0.0,0.0 +0,0.839800,0.0,0.0,0.0,0.0,0.0 +0,0.839900,0.0,0.0,0.0,0.0,0.0 +0,0.840000,0.0,0.0,0.0,0.0,0.0 +0,0.840100,0.0,0.0,0.0,0.0,0.0 +1,247.092323,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.840200,0.0,0.0,0.0,0.0,0.0 +0,0.840300,0.0,0.0,0.0,0.0,0.0 +0,0.840400,0.0,0.0,0.0,0.0,0.0 +0,0.840500,0.0,0.0,0.0,0.0,0.0 +0,0.840600,0.0,0.0,0.0,0.0,0.0 +0,0.840700,0.0,0.0,0.0,0.0,0.0 +0,0.840800,0.0,0.0,0.0,0.0,0.0 +0,0.840900,0.0,0.0,0.0,0.0,0.0 +0,0.841000,0.0,0.0,0.0,0.0,0.0 +0,0.841100,0.0,0.0,0.0,0.0,0.0 +0,0.841200,0.0,0.0,0.0,0.0,0.0 +0,0.841300,0.0,0.0,0.0,0.0,0.0 +0,0.841400,0.0,0.0,0.0,0.0,0.0 +0,0.841500,0.0,0.0,0.0,0.0,0.0 +0,0.841600,0.0,0.0,0.0,0.0,0.0 +0,0.841700,0.0,0.0,0.0,0.0,0.0 +0,0.841800,0.0,0.0,0.0,0.0,0.0 +0,0.841900,0.0,0.0,0.0,0.0,0.0 +0,0.842000,0.0,0.0,0.0,0.0,0.0 +0,0.842100,0.0,0.0,0.0,0.0,0.0 +0,0.842200,0.0,0.0,0.0,0.0,0.0 +0,0.842300,0.0,0.0,0.0,0.0,0.0 +0,0.842400,0.0,0.0,0.0,0.0,0.0 +0,0.842500,0.0,0.0,0.0,0.0,0.0 +0,0.842600,0.0,0.0,0.0,0.0,0.0 +0,0.842700,0.0,0.0,0.0,0.0,0.0 +0,0.842800,0.0,0.0,0.0,0.0,0.0 +0,0.842900,0.0,0.0,0.0,0.0,0.0 +0,0.843000,0.0,0.0,0.0,0.0,0.0 +0,0.843100,0.0,0.0,0.0,0.0,0.0 +0,0.843200,0.0,0.0,0.0,0.0,0.0 +0,0.843300,0.0,0.0,0.0,0.0,0.0 +0,0.843400,0.0,0.0,0.0,0.0,0.0 +0,0.843500,0.0,0.0,0.0,0.0,0.0 +0,0.843600,0.0,0.0,0.0,0.0,0.0 +0,0.843700,0.0,0.0,0.0,0.0,0.0 +0,0.843800,0.0,0.0,0.0,0.0,0.0 +0,0.843900,0.0,0.0,0.0,0.0,0.0 +0,0.844000,0.0,0.0,0.0,0.0,0.0 +0,0.844100,0.0,0.0,0.0,0.0,0.0 +0,0.844200,0.0,0.0,0.0,0.0,0.0 +0,0.844300,0.0,0.0,0.0,0.0,0.0 +0,0.844400,0.0,0.0,0.0,0.0,0.0 +0,0.844500,0.0,0.0,0.0,0.0,0.0 +0,0.844600,0.0,0.0,0.0,0.0,0.0 +0,0.844700,0.0,0.0,0.0,0.0,0.0 +0,0.844800,0.0,0.0,0.0,0.0,0.0 +0,0.844900,0.0,0.0,0.0,0.0,0.0 +0,0.845000,0.0,0.0,0.0,0.0,0.0 +0,0.845100,0.0,0.0,0.0,0.0,0.0 +0,0.845200,0.0,0.0,0.0,0.0,0.0 +0,0.845300,0.0,0.0,0.0,0.0,0.0 +0,0.845400,0.0,0.0,0.0,0.0,0.0 +0,0.845500,0.0,0.0,0.0,0.0,0.0 +0,0.845600,0.0,0.0,0.0,0.0,0.0 +0,0.845700,0.0,0.0,0.0,0.0,0.0 +0,0.845800,0.0,0.0,0.0,0.0,0.0 +0,0.845900,0.0,0.0,0.0,0.0,0.0 +0,0.846000,0.0,0.0,0.0,0.0,0.0 +0,0.846100,0.0,0.0,0.0,0.0,0.0 +0,0.846200,0.0,0.0,0.0,0.0,0.0 +0,0.846300,0.0,0.0,0.0,0.0,0.0 +0,0.846400,0.0,0.0,0.0,0.0,0.0 +0,0.846500,0.0,0.0,0.0,0.0,0.0 +0,0.846600,0.0,0.0,0.0,0.0,0.0 +0,0.846700,0.0,0.0,0.0,0.0,0.0 +0,0.846800,0.0,0.0,0.0,0.0,0.0 +0,0.846900,0.0,0.0,0.0,0.0,0.0 +0,0.847000,0.0,0.0,0.0,0.0,0.0 +0,0.847100,0.0,0.0,0.0,0.0,0.0 +0,0.847200,0.0,0.0,0.0,0.0,0.0 +0,0.847300,0.0,0.0,0.0,0.0,0.0 +0,0.847400,0.0,0.0,0.0,0.0,0.0 +0,0.847500,0.0,0.0,0.0,0.0,0.0 +0,0.847600,0.0,0.0,0.0,0.0,0.0 +0,0.847700,0.0,0.0,0.0,0.0,0.0 +0,0.847800,0.0,0.0,0.0,0.0,0.0 +0,0.847900,0.0,0.0,0.0,0.0,0.0 +0,0.848000,0.0,0.0,0.0,0.0,0.0 +0,0.848100,0.0,0.0,0.0,0.0,0.0 +0,0.848200,0.0,0.0,0.0,0.0,0.0 +0,0.848300,0.0,0.0,0.0,0.0,0.0 +0,0.848400,0.0,0.0,0.0,0.0,0.0 +0,0.848500,0.0,0.0,0.0,0.0,0.0 +0,0.848600,0.0,0.0,0.0,0.0,0.0 +0,0.848700,0.0,0.0,0.0,0.0,0.0 +0,0.848800,0.0,0.0,0.0,0.0,0.0 +0,0.848900,0.0,0.0,0.0,0.0,0.0 +0,0.849000,0.0,0.0,0.0,0.0,0.0 +0,0.849100,0.0,0.0,0.0,0.0,0.0 +0,0.849200,0.0,0.0,0.0,0.0,0.0 +0,0.849300,0.0,0.0,0.0,0.0,0.0 +0,0.849400,0.0,0.0,0.0,0.0,0.0 +0,0.849500,0.0,0.0,0.0,0.0,0.0 +0,0.849600,0.0,0.0,0.0,0.0,0.0 +0,0.849700,0.0,0.0,0.0,0.0,0.0 +0,0.849800,0.0,0.0,0.0,0.0,0.0 +0,0.849900,0.0,0.0,0.0,0.0,0.0 +0,0.850000,0.0,0.0,0.0,0.0,0.0 +0,0.850100,0.0,0.0,0.0,0.0,0.0 +0,0.850200,0.0,0.0,0.0,0.0,0.0 +0,0.850300,0.0,0.0,0.0,0.0,0.0 +0,0.850400,0.0,0.0,0.0,0.0,0.0 +0,0.850500,0.0,0.0,0.0,0.0,0.0 +0,0.850600,0.0,0.0,0.0,0.0,0.0 +0,0.850700,0.0,0.0,0.0,0.0,0.0 +0,0.850800,0.0,0.0,0.0,0.0,0.0 +0,0.850900,0.0,0.0,0.0,0.0,0.0 +0,0.851000,0.0,0.0,0.0,0.0,0.0 +0,0.851100,0.0,0.0,0.0,0.0,0.0 +0,0.851200,0.0,0.0,0.0,0.0,0.0 +0,0.851300,0.0,0.0,0.0,0.0,0.0 +0,0.851400,0.0,0.0,0.0,0.0,0.0 +0,0.851500,0.0,0.0,0.0,0.0,0.0 +0,0.851600,0.0,0.0,0.0,0.0,0.0 +0,0.851700,0.0,0.0,0.0,0.0,0.0 +0,0.851800,0.0,0.0,0.0,0.0,0.0 +0,0.851900,0.0,0.0,0.0,0.0,0.0 +0,0.852000,0.0,0.0,0.0,0.0,0.0 +0,0.852100,0.0,0.0,0.0,0.0,0.0 +0,0.852200,0.0,0.0,0.0,0.0,0.0 +0,0.852300,0.0,0.0,0.0,0.0,0.0 +0,0.852400,0.0,0.0,0.0,0.0,0.0 +0,0.852500,0.0,0.0,0.0,0.0,0.0 +0,0.852600,0.0,0.0,0.0,0.0,0.0 +0,0.852700,0.0,0.0,0.0,0.0,0.0 +0,0.852800,0.0,0.0,0.0,0.0,0.0 +0,0.852900,0.0,0.0,0.0,0.0,0.0 +0,0.853000,0.0,0.0,0.0,0.0,0.0 +0,0.853100,0.0,0.0,0.0,0.0,0.0 +0,0.853200,0.0,0.0,0.0,0.0,0.0 +0,0.853300,0.0,0.0,0.0,0.0,0.0 +0,0.853400,0.0,0.0,0.0,0.0,0.0 +0,0.853500,0.0,0.0,0.0,0.0,0.0 +0,0.853600,0.0,0.0,0.0,0.0,0.0 +0,0.853700,0.0,0.0,0.0,0.0,0.0 +0,0.853800,0.0,0.0,0.0,0.0,0.0 +0,0.853900,0.0,0.0,0.0,0.0,0.0 +0,0.854000,0.0,0.0,0.0,0.0,0.0 +0,0.854100,0.0,0.0,0.0,0.0,0.0 +0,0.854200,0.0,0.0,0.0,0.0,0.0 +0,0.854300,0.0,0.0,0.0,0.0,0.0 +0,0.854400,0.0,0.0,0.0,0.0,0.0 +0,0.854500,0.0,0.0,0.0,0.0,0.0 +0,0.854600,0.0,0.0,0.0,0.0,0.0 +0,0.854700,0.0,0.0,0.0,0.0,0.0 +0,0.854800,0.0,0.0,0.0,0.0,0.0 +0,0.854900,0.0,0.0,0.0,0.0,0.0 +0,0.855000,0.0,0.0,0.0,0.0,0.0 +0,0.855100,0.0,0.0,0.0,0.0,0.0 +0,0.855200,0.0,0.0,0.0,0.0,0.0 +0,0.855300,0.0,0.0,0.0,0.0,0.0 +0,0.855400,0.0,0.0,0.0,0.0,0.0 +0,0.855500,0.0,0.0,0.0,0.0,0.0 +0,0.855600,0.0,0.0,0.0,0.0,0.0 +0,0.855700,0.0,0.0,0.0,0.0,0.0 +0,0.855800,0.0,0.0,0.0,0.0,0.0 +0,0.855900,0.0,0.0,0.0,0.0,0.0 +0,0.856000,0.0,0.0,0.0,0.0,0.0 +0,0.856100,0.0,0.0,0.0,0.0,0.0 +0,0.856200,0.0,0.0,0.0,0.0,0.0 +0,0.856300,0.0,0.0,0.0,0.0,0.0 +0,0.856400,0.0,0.0,0.0,0.0,0.0 +0,0.856500,0.0,0.0,0.0,0.0,0.0 +0,0.856600,0.0,0.0,0.0,0.0,0.0 +0,0.856700,0.0,0.0,0.0,0.0,0.0 +0,0.856800,0.0,0.0,0.0,0.0,0.0 +0,0.856900,0.0,0.0,0.0,0.0,0.0 +0,0.857000,0.0,0.0,0.0,0.0,0.0 +0,0.857100,0.0,0.0,0.0,0.0,0.0 +0,0.857200,0.0,0.0,0.0,0.0,0.0 +0,0.857300,0.0,0.0,0.0,0.0,0.0 +0,0.857400,0.0,0.0,0.0,0.0,0.0 +0,0.857500,0.0,0.0,0.0,0.0,0.0 +0,0.857600,0.0,0.0,0.0,0.0,0.0 +0,0.857700,0.0,0.0,0.0,0.0,0.0 +0,0.857800,0.0,0.0,0.0,0.0,0.0 +0,0.857900,0.0,0.0,0.0,0.0,0.0 +0,0.858000,0.0,0.0,0.0,0.0,0.0 +0,0.858100,0.0,0.0,0.0,0.0,0.0 +0,0.858200,0.0,0.0,0.0,0.0,0.0 +0,0.858300,0.0,0.0,0.0,0.0,0.0 +0,0.858400,0.0,0.0,0.0,0.0,0.0 +0,0.858500,0.0,0.0,0.0,0.0,0.0 +0,0.858600,0.0,0.0,0.0,0.0,0.0 +0,0.858700,0.0,0.0,0.0,0.0,0.0 +0,0.858800,0.0,0.0,0.0,0.0,0.0 +0,0.858900,0.0,0.0,0.0,0.0,0.0 +0,0.859000,0.0,0.0,0.0,0.0,0.0 +0,0.859100,0.0,0.0,0.0,0.0,0.0 +0,0.859200,0.0,0.0,0.0,0.0,0.0 +0,0.859300,0.0,0.0,0.0,0.0,0.0 +0,0.859400,0.0,0.0,0.0,0.0,0.0 +0,0.859500,0.0,0.0,0.0,0.0,0.0 +0,0.859600,0.0,0.0,0.0,0.0,0.0 +0,0.859700,0.0,0.0,0.0,0.0,0.0 +0,0.859800,0.0,0.0,0.0,0.0,0.0 +0,0.859900,0.0,0.0,0.0,0.0,0.0 +0,0.860000,0.0,0.0,0.0,0.0,0.0 +0,0.860100,0.0,0.0,0.0,0.0,0.0 +1,265.162032,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.860200,0.0,0.0,0.0,0.0,0.0 +0,0.860300,0.0,0.0,0.0,0.0,0.0 +0,0.860400,0.0,0.0,0.0,0.0,0.0 +0,0.860500,0.0,0.0,0.0,0.0,0.0 +0,0.860600,0.0,0.0,0.0,0.0,0.0 +0,0.860700,0.0,0.0,0.0,0.0,0.0 +0,0.860800,0.0,0.0,0.0,0.0,0.0 +0,0.860900,0.0,0.0,0.0,0.0,0.0 +0,0.861000,0.0,0.0,0.0,0.0,0.0 +0,0.861100,0.0,0.0,0.0,0.0,0.0 +0,0.861200,0.0,0.0,0.0,0.0,0.0 +0,0.861300,0.0,0.0,0.0,0.0,0.0 +0,0.861400,0.0,0.0,0.0,0.0,0.0 +0,0.861500,0.0,0.0,0.0,0.0,0.0 +0,0.861600,0.0,0.0,0.0,0.0,0.0 +0,0.861700,0.0,0.0,0.0,0.0,0.0 +0,0.861800,0.0,0.0,0.0,0.0,0.0 +0,0.861900,0.0,0.0,0.0,0.0,0.0 +0,0.862000,0.0,0.0,0.0,0.0,0.0 +0,0.862100,0.0,0.0,0.0,0.0,0.0 +0,0.862200,0.0,0.0,0.0,0.0,0.0 +0,0.862300,0.0,0.0,0.0,0.0,0.0 +0,0.862400,0.0,0.0,0.0,0.0,0.0 +0,0.862500,0.0,0.0,0.0,0.0,0.0 +0,0.862600,0.0,0.0,0.0,0.0,0.0 +0,0.862700,0.0,0.0,0.0,0.0,0.0 +0,0.862800,0.0,0.0,0.0,0.0,0.0 +0,0.862900,0.0,0.0,0.0,0.0,0.0 +0,0.863000,0.0,0.0,0.0,0.0,0.0 +0,0.863100,0.0,0.0,0.0,0.0,0.0 +0,0.863200,0.0,0.0,0.0,0.0,0.0 +0,0.863300,0.0,0.0,0.0,0.0,0.0 +0,0.863400,0.0,0.0,0.0,0.0,0.0 +0,0.863500,0.0,0.0,0.0,0.0,0.0 +0,0.863600,0.0,0.0,0.0,0.0,0.0 +0,0.863700,0.0,0.0,0.0,0.0,0.0 +0,0.863800,0.0,0.0,0.0,0.0,0.0 +0,0.863900,0.0,0.0,0.0,0.0,0.0 +0,0.864000,0.0,0.0,0.0,0.0,0.0 +0,0.864100,0.0,0.0,0.0,0.0,0.0 +0,0.864200,0.0,0.0,0.0,0.0,0.0 +0,0.864300,0.0,0.0,0.0,0.0,0.0 +0,0.864400,0.0,0.0,0.0,0.0,0.0 +0,0.864500,0.0,0.0,0.0,0.0,0.0 +0,0.864600,0.0,0.0,0.0,0.0,0.0 +0,0.864700,0.0,0.0,0.0,0.0,0.0 +0,0.864800,0.0,0.0,0.0,0.0,0.0 +0,0.864900,0.0,0.0,0.0,0.0,0.0 +0,0.865000,0.0,0.0,0.0,0.0,0.0 +0,0.865100,0.0,0.0,0.0,0.0,0.0 +0,0.865200,0.0,0.0,0.0,0.0,0.0 +0,0.865300,0.0,0.0,0.0,0.0,0.0 +0,0.865400,0.0,0.0,0.0,0.0,0.0 +0,0.865500,0.0,0.0,0.0,0.0,0.0 +0,0.865600,0.0,0.0,0.0,0.0,0.0 +0,0.865700,0.0,0.0,0.0,0.0,0.0 +0,0.865800,0.0,0.0,0.0,0.0,0.0 +0,0.865900,0.0,0.0,0.0,0.0,0.0 +0,0.866000,0.0,0.0,0.0,0.0,0.0 +0,0.866100,0.0,0.0,0.0,0.0,0.0 +0,0.866200,0.0,0.0,0.0,0.0,0.0 +0,0.866300,0.0,0.0,0.0,0.0,0.0 +0,0.866400,0.0,0.0,0.0,0.0,0.0 +0,0.866500,0.0,0.0,0.0,0.0,0.0 +0,0.866600,0.0,0.0,0.0,0.0,0.0 +0,0.866700,0.0,0.0,0.0,0.0,0.0 +0,0.866800,0.0,0.0,0.0,0.0,0.0 +0,0.866900,0.0,0.0,0.0,0.0,0.0 +0,0.867000,0.0,0.0,0.0,0.0,0.0 +0,0.867100,0.0,0.0,0.0,0.0,0.0 +0,0.867200,0.0,0.0,0.0,0.0,0.0 +0,0.867300,0.0,0.0,0.0,0.0,0.0 +0,0.867400,0.0,0.0,0.0,0.0,0.0 +0,0.867500,0.0,0.0,0.0,0.0,0.0 +0,0.867600,0.0,0.0,0.0,0.0,0.0 +0,0.867700,0.0,0.0,0.0,0.0,0.0 +0,0.867800,0.0,0.0,0.0,0.0,0.0 +0,0.867900,0.0,0.0,0.0,0.0,0.0 +0,0.868000,0.0,0.0,0.0,0.0,0.0 +0,0.868100,0.0,0.0,0.0,0.0,0.0 +0,0.868200,0.0,0.0,0.0,0.0,0.0 +0,0.868300,0.0,0.0,0.0,0.0,0.0 +0,0.868400,0.0,0.0,0.0,0.0,0.0 +0,0.868500,0.0,0.0,0.0,0.0,0.0 +0,0.868600,0.0,0.0,0.0,0.0,0.0 +0,0.868700,0.0,0.0,0.0,0.0,0.0 +0,0.868800,0.0,0.0,0.0,0.0,0.0 +0,0.868900,0.0,0.0,0.0,0.0,0.0 +0,0.869000,0.0,0.0,0.0,0.0,0.0 +0,0.869100,0.0,0.0,0.0,0.0,0.0 +0,0.869200,0.0,0.0,0.0,0.0,0.0 +0,0.869300,0.0,0.0,0.0,0.0,0.0 +0,0.869400,0.0,0.0,0.0,0.0,0.0 +0,0.869500,0.0,0.0,0.0,0.0,0.0 +0,0.869600,0.0,0.0,0.0,0.0,0.0 +0,0.869700,0.0,0.0,0.0,0.0,0.0 +0,0.869800,0.0,0.0,0.0,0.0,0.0 +0,0.869900,0.0,0.0,0.0,0.0,0.0 +0,0.870000,0.0,0.0,0.0,0.0,0.0 +0,0.870100,0.0,0.0,0.0,0.0,0.0 +0,0.870200,0.0,0.0,0.0,0.0,0.0 +0,0.870300,0.0,0.0,0.0,0.0,0.0 +0,0.870400,0.0,0.0,0.0,0.0,0.0 +0,0.870500,0.0,0.0,0.0,0.0,0.0 +0,0.870600,0.0,0.0,0.0,0.0,0.0 +0,0.870700,0.0,0.0,0.0,0.0,0.0 +0,0.870800,0.0,0.0,0.0,0.0,0.0 +0,0.870900,0.0,0.0,0.0,0.0,0.0 +0,0.871000,0.0,0.0,0.0,0.0,0.0 +0,0.871100,0.0,0.0,0.0,0.0,0.0 +0,0.871200,0.0,0.0,0.0,0.0,0.0 +0,0.871300,0.0,0.0,0.0,0.0,0.0 +0,0.871400,0.0,0.0,0.0,0.0,0.0 +0,0.871500,0.0,0.0,0.0,0.0,0.0 +0,0.871600,0.0,0.0,0.0,0.0,0.0 +0,0.871700,0.0,0.0,0.0,0.0,0.0 +0,0.871800,0.0,0.0,0.0,0.0,0.0 +0,0.871900,0.0,0.0,0.0,0.0,0.0 +0,0.872000,0.0,0.0,0.0,0.0,0.0 +0,0.872100,0.0,0.0,0.0,0.0,0.0 +0,0.872200,0.0,0.0,0.0,0.0,0.0 +0,0.872300,0.0,0.0,0.0,0.0,0.0 +0,0.872400,0.0,0.0,0.0,0.0,0.0 +0,0.872500,0.0,0.0,0.0,0.0,0.0 +0,0.872600,0.0,0.0,0.0,0.0,0.0 +0,0.872700,0.0,0.0,0.0,0.0,0.0 +0,0.872800,0.0,0.0,0.0,0.0,0.0 +0,0.872900,0.0,0.0,0.0,0.0,0.0 +0,0.873000,0.0,0.0,0.0,0.0,0.0 +0,0.873100,0.0,0.0,0.0,0.0,0.0 +0,0.873200,0.0,0.0,0.0,0.0,0.0 +0,0.873300,0.0,0.0,0.0,0.0,0.0 +0,0.873400,0.0,0.0,0.0,0.0,0.0 +0,0.873500,0.0,0.0,0.0,0.0,0.0 +0,0.873600,0.0,0.0,0.0,0.0,0.0 +0,0.873700,0.0,0.0,0.0,0.0,0.0 +0,0.873800,0.0,0.0,0.0,0.0,0.0 +0,0.873900,0.0,0.0,0.0,0.0,0.0 +0,0.874000,0.0,0.0,0.0,0.0,0.0 +0,0.874100,0.0,0.0,0.0,0.0,0.0 +0,0.874200,0.0,0.0,0.0,0.0,0.0 +0,0.874300,0.0,0.0,0.0,0.0,0.0 +0,0.874400,0.0,0.0,0.0,0.0,0.0 +0,0.874500,0.0,0.0,0.0,0.0,0.0 +0,0.874600,0.0,0.0,0.0,0.0,0.0 +0,0.874700,0.0,0.0,0.0,0.0,0.0 +0,0.874800,0.0,0.0,0.0,0.0,0.0 +0,0.874900,0.0,0.0,0.0,0.0,0.0 +0,0.875000,0.0,0.0,0.0,0.0,0.0 +0,0.875100,0.0,0.0,0.0,0.0,0.0 +0,0.875200,0.0,0.0,0.0,0.0,0.0 +0,0.875300,0.0,0.0,0.0,0.0,0.0 +0,0.875400,0.0,0.0,0.0,0.0,0.0 +0,0.875500,0.0,0.0,0.0,0.0,0.0 +0,0.875600,0.0,0.0,0.0,0.0,0.0 +0,0.875700,0.0,0.0,0.0,0.0,0.0 +0,0.875800,0.0,0.0,0.0,0.0,0.0 +0,0.875900,0.0,0.0,0.0,0.0,0.0 +0,0.876000,0.0,0.0,0.0,0.0,0.0 +0,0.876100,0.0,0.0,0.0,0.0,0.0 +0,0.876200,0.0,0.0,0.0,0.0,0.0 +0,0.876300,0.0,0.0,0.0,0.0,0.0 +0,0.876400,0.0,0.0,0.0,0.0,0.0 +0,0.876500,0.0,0.0,0.0,0.0,0.0 +0,0.876600,0.0,0.0,0.0,0.0,0.0 +0,0.876700,0.0,0.0,0.0,0.0,0.0 +0,0.876800,0.0,0.0,0.0,0.0,0.0 +0,0.876900,0.0,0.0,0.0,0.0,0.0 +0,0.877000,0.0,0.0,0.0,0.0,0.0 +0,0.877100,0.0,0.0,0.0,0.0,0.0 +0,0.877200,0.0,0.0,0.0,0.0,0.0 +0,0.877300,0.0,0.0,0.0,0.0,0.0 +0,0.877400,0.0,0.0,0.0,0.0,0.0 +0,0.877500,0.0,0.0,0.0,0.0,0.0 +0,0.877600,0.0,0.0,0.0,0.0,0.0 +0,0.877700,0.0,0.0,0.0,0.0,0.0 +0,0.877800,0.0,0.0,0.0,0.0,0.0 +0,0.877900,0.0,0.0,0.0,0.0,0.0 +0,0.878000,0.0,0.0,0.0,0.0,0.0 +0,0.878100,0.0,0.0,0.0,0.0,0.0 +0,0.878200,0.0,0.0,0.0,0.0,0.0 +0,0.878300,0.0,0.0,0.0,0.0,0.0 +0,0.878400,0.0,0.0,0.0,0.0,0.0 +0,0.878500,0.0,0.0,0.0,0.0,0.0 +0,0.878600,0.0,0.0,0.0,0.0,0.0 +0,0.878700,0.0,0.0,0.0,0.0,0.0 +0,0.878800,0.0,0.0,0.0,0.0,0.0 +0,0.878900,0.0,0.0,0.0,0.0,0.0 +0,0.879000,0.0,0.0,0.0,0.0,0.0 +0,0.879100,0.0,0.0,0.0,0.0,0.0 +0,0.879200,0.0,0.0,0.0,0.0,0.0 +0,0.879300,0.0,0.0,0.0,0.0,0.0 +0,0.879400,0.0,0.0,0.0,0.0,0.0 +0,0.879500,0.0,0.0,0.0,0.0,0.0 +0,0.879600,0.0,0.0,0.0,0.0,0.0 +0,0.879700,0.0,0.0,0.0,0.0,0.0 +0,0.879800,0.0,0.0,0.0,0.0,0.0 +0,0.879900,0.0,0.0,0.0,0.0,0.0 +0,0.880000,0.0,0.0,0.0,0.0,0.0 +0,0.880100,0.0,0.0,0.0,0.0,0.0 +1,284.091891,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.880200,0.0,0.0,0.0,0.0,0.0 +0,0.880300,0.0,0.0,0.0,0.0,0.0 +0,0.880400,0.0,0.0,0.0,0.0,0.0 +0,0.880500,0.0,0.0,0.0,0.0,0.0 +0,0.880600,0.0,0.0,0.0,0.0,0.0 +0,0.880700,0.0,0.0,0.0,0.0,0.0 +0,0.880800,0.0,0.0,0.0,0.0,0.0 +0,0.880900,0.0,0.0,0.0,0.0,0.0 +0,0.881000,0.0,0.0,0.0,0.0,0.0 +0,0.881100,0.0,0.0,0.0,0.0,0.0 +0,0.881200,0.0,0.0,0.0,0.0,0.0 +0,0.881300,0.0,0.0,0.0,0.0,0.0 +0,0.881400,0.0,0.0,0.0,0.0,0.0 +0,0.881500,0.0,0.0,0.0,0.0,0.0 +0,0.881600,0.0,0.0,0.0,0.0,0.0 +0,0.881700,0.0,0.0,0.0,0.0,0.0 +0,0.881800,0.0,0.0,0.0,0.0,0.0 +0,0.881900,0.0,0.0,0.0,0.0,0.0 +0,0.882000,0.0,0.0,0.0,0.0,0.0 +0,0.882100,0.0,0.0,0.0,0.0,0.0 +0,0.882200,0.0,0.0,0.0,0.0,0.0 +0,0.882300,0.0,0.0,0.0,0.0,0.0 +0,0.882400,0.0,0.0,0.0,0.0,0.0 +0,0.882500,0.0,0.0,0.0,0.0,0.0 +0,0.882600,0.0,0.0,0.0,0.0,0.0 +0,0.882700,0.0,0.0,0.0,0.0,0.0 +0,0.882800,0.0,0.0,0.0,0.0,0.0 +0,0.882900,0.0,0.0,0.0,0.0,0.0 +0,0.883000,0.0,0.0,0.0,0.0,0.0 +0,0.883100,0.0,0.0,0.0,0.0,0.0 +0,0.883200,0.0,0.0,0.0,0.0,0.0 +0,0.883300,0.0,0.0,0.0,0.0,0.0 +0,0.883400,0.0,0.0,0.0,0.0,0.0 +0,0.883500,0.0,0.0,0.0,0.0,0.0 +0,0.883600,0.0,0.0,0.0,0.0,0.0 +0,0.883700,0.0,0.0,0.0,0.0,0.0 +0,0.883800,0.0,0.0,0.0,0.0,0.0 +0,0.883900,0.0,0.0,0.0,0.0,0.0 +0,0.884000,0.0,0.0,0.0,0.0,0.0 +0,0.884100,0.0,0.0,0.0,0.0,0.0 +0,0.884200,0.0,0.0,0.0,0.0,0.0 +0,0.884300,0.0,0.0,0.0,0.0,0.0 +0,0.884400,0.0,0.0,0.0,0.0,0.0 +0,0.884500,0.0,0.0,0.0,0.0,0.0 +0,0.884600,0.0,0.0,0.0,0.0,0.0 +0,0.884700,0.0,0.0,0.0,0.0,0.0 +0,0.884800,0.0,0.0,0.0,0.0,0.0 +0,0.884900,0.0,0.0,0.0,0.0,0.0 +0,0.885000,0.0,0.0,0.0,0.0,0.0 +0,0.885100,0.0,0.0,0.0,0.0,0.0 +0,0.885200,0.0,0.0,0.0,0.0,0.0 +0,0.885300,0.0,0.0,0.0,0.0,0.0 +0,0.885400,0.0,0.0,0.0,0.0,0.0 +0,0.885500,0.0,0.0,0.0,0.0,0.0 +0,0.885600,0.0,0.0,0.0,0.0,0.0 +0,0.885700,0.0,0.0,0.0,0.0,0.0 +0,0.885800,0.0,0.0,0.0,0.0,0.0 +0,0.885900,0.0,0.0,0.0,0.0,0.0 +0,0.886000,0.0,0.0,0.0,0.0,0.0 +0,0.886100,0.0,0.0,0.0,0.0,0.0 +0,0.886200,0.0,0.0,0.0,0.0,0.0 +0,0.886300,0.0,0.0,0.0,0.0,0.0 +0,0.886400,0.0,0.0,0.0,0.0,0.0 +0,0.886500,0.0,0.0,0.0,0.0,0.0 +0,0.886600,0.0,0.0,0.0,0.0,0.0 +0,0.886700,0.0,0.0,0.0,0.0,0.0 +0,0.886800,0.0,0.0,0.0,0.0,0.0 +0,0.886900,0.0,0.0,0.0,0.0,0.0 +0,0.887000,0.0,0.0,0.0,0.0,0.0 +0,0.887100,0.0,0.0,0.0,0.0,0.0 +0,0.887200,0.0,0.0,0.0,0.0,0.0 +0,0.887300,0.0,0.0,0.0,0.0,0.0 +0,0.887400,0.0,0.0,0.0,0.0,0.0 +0,0.887500,0.0,0.0,0.0,0.0,0.0 +0,0.887600,0.0,0.0,0.0,0.0,0.0 +0,0.887700,0.0,0.0,0.0,0.0,0.0 +0,0.887800,0.0,0.0,0.0,0.0,0.0 +0,0.887900,0.0,0.0,0.0,0.0,0.0 +0,0.888000,0.0,0.0,0.0,0.0,0.0 +0,0.888100,0.0,0.0,0.0,0.0,0.0 +0,0.888200,0.0,0.0,0.0,0.0,0.0 +0,0.888300,0.0,0.0,0.0,0.0,0.0 +0,0.888400,0.0,0.0,0.0,0.0,0.0 +0,0.888500,0.0,0.0,0.0,0.0,0.0 +0,0.888600,0.0,0.0,0.0,0.0,0.0 +0,0.888700,0.0,0.0,0.0,0.0,0.0 +0,0.888800,0.0,0.0,0.0,0.0,0.0 +0,0.888900,0.0,0.0,0.0,0.0,0.0 +0,0.889000,0.0,0.0,0.0,0.0,0.0 +0,0.889100,0.0,0.0,0.0,0.0,0.0 +0,0.889200,0.0,0.0,0.0,0.0,0.0 +0,0.889300,0.0,0.0,0.0,0.0,0.0 +0,0.889400,0.0,0.0,0.0,0.0,0.0 +0,0.889500,0.0,0.0,0.0,0.0,0.0 +0,0.889600,0.0,0.0,0.0,0.0,0.0 +0,0.889700,0.0,0.0,0.0,0.0,0.0 +0,0.889800,0.0,0.0,0.0,0.0,0.0 +0,0.889900,0.0,0.0,0.0,0.0,0.0 +0,0.890000,0.0,0.0,0.0,0.0,0.0 +0,0.890100,0.0,0.0,0.0,0.0,0.0 +0,0.890200,0.0,0.0,0.0,0.0,0.0 +0,0.890300,0.0,0.0,0.0,0.0,0.0 +0,0.890400,0.0,0.0,0.0,0.0,0.0 +0,0.890500,0.0,0.0,0.0,0.0,0.0 +0,0.890600,0.0,0.0,0.0,0.0,0.0 +0,0.890700,0.0,0.0,0.0,0.0,0.0 +0,0.890800,0.0,0.0,0.0,0.0,0.0 +0,0.890900,0.0,0.0,0.0,0.0,0.0 +0,0.891000,0.0,0.0,0.0,0.0,0.0 +0,0.891100,0.0,0.0,0.0,0.0,0.0 +0,0.891200,0.0,0.0,0.0,0.0,0.0 +0,0.891300,0.0,0.0,0.0,0.0,0.0 +0,0.891400,0.0,0.0,0.0,0.0,0.0 +0,0.891500,0.0,0.0,0.0,0.0,0.0 +0,0.891600,0.0,0.0,0.0,0.0,0.0 +0,0.891700,0.0,0.0,0.0,0.0,0.0 +0,0.891800,0.0,0.0,0.0,0.0,0.0 +0,0.891900,0.0,0.0,0.0,0.0,0.0 +0,0.892000,0.0,0.0,0.0,0.0,0.0 +0,0.892100,0.0,0.0,0.0,0.0,0.0 +0,0.892200,0.0,0.0,0.0,0.0,0.0 +0,0.892300,0.0,0.0,0.0,0.0,0.0 +0,0.892400,0.0,0.0,0.0,0.0,0.0 +0,0.892500,0.0,0.0,0.0,0.0,0.0 +0,0.892600,0.0,0.0,0.0,0.0,0.0 +0,0.892700,0.0,0.0,0.0,0.0,0.0 +0,0.892800,0.0,0.0,0.0,0.0,0.0 +0,0.892900,0.0,0.0,0.0,0.0,0.0 +0,0.893000,0.0,0.0,0.0,0.0,0.0 +0,0.893100,0.0,0.0,0.0,0.0,0.0 +0,0.893200,0.0,0.0,0.0,0.0,0.0 +0,0.893300,0.0,0.0,0.0,0.0,0.0 +0,0.893400,0.0,0.0,0.0,0.0,0.0 +0,0.893500,0.0,0.0,0.0,0.0,0.0 +0,0.893600,0.0,0.0,0.0,0.0,0.0 +0,0.893700,0.0,0.0,0.0,0.0,0.0 +0,0.893800,0.0,0.0,0.0,0.0,0.0 +0,0.893900,0.0,0.0,0.0,0.0,0.0 +0,0.894000,0.0,0.0,0.0,0.0,0.0 +0,0.894100,0.0,0.0,0.0,0.0,0.0 +0,0.894200,0.0,0.0,0.0,0.0,0.0 +0,0.894300,0.0,0.0,0.0,0.0,0.0 +0,0.894400,0.0,0.0,0.0,0.0,0.0 +0,0.894500,0.0,0.0,0.0,0.0,0.0 +0,0.894600,0.0,0.0,0.0,0.0,0.0 +0,0.894700,0.0,0.0,0.0,0.0,0.0 +0,0.894800,0.0,0.0,0.0,0.0,0.0 +0,0.894900,0.0,0.0,0.0,0.0,0.0 +0,0.895000,0.0,0.0,0.0,0.0,0.0 +0,0.895100,0.0,0.0,0.0,0.0,0.0 +0,0.895200,0.0,0.0,0.0,0.0,0.0 +0,0.895300,0.0,0.0,0.0,0.0,0.0 +0,0.895400,0.0,0.0,0.0,0.0,0.0 +0,0.895500,0.0,0.0,0.0,0.0,0.0 +0,0.895600,0.0,0.0,0.0,0.0,0.0 +0,0.895700,0.0,0.0,0.0,0.0,0.0 +0,0.895800,0.0,0.0,0.0,0.0,0.0 +0,0.895900,0.0,0.0,0.0,0.0,0.0 +0,0.896000,0.0,0.0,0.0,0.0,0.0 +0,0.896100,0.0,0.0,0.0,0.0,0.0 +0,0.896200,0.0,0.0,0.0,0.0,0.0 +0,0.896300,0.0,0.0,0.0,0.0,0.0 +0,0.896400,0.0,0.0,0.0,0.0,0.0 +0,0.896500,0.0,0.0,0.0,0.0,0.0 +0,0.896600,0.0,0.0,0.0,0.0,0.0 +0,0.896700,0.0,0.0,0.0,0.0,0.0 +0,0.896800,0.0,0.0,0.0,0.0,0.0 +0,0.896900,0.0,0.0,0.0,0.0,0.0 +0,0.897000,0.0,0.0,0.0,0.0,0.0 +0,0.897100,0.0,0.0,0.0,0.0,0.0 +0,0.897200,0.0,0.0,0.0,0.0,0.0 +0,0.897300,0.0,0.0,0.0,0.0,0.0 +0,0.897400,0.0,0.0,0.0,0.0,0.0 +0,0.897500,0.0,0.0,0.0,0.0,0.0 +0,0.897600,0.0,0.0,0.0,0.0,0.0 +0,0.897700,0.0,0.0,0.0,0.0,0.0 +0,0.897800,0.0,0.0,0.0,0.0,0.0 +0,0.897900,0.0,0.0,0.0,0.0,0.0 +0,0.898000,0.0,0.0,0.0,0.0,0.0 +0,0.898100,0.0,0.0,0.0,0.0,0.0 +0,0.898200,0.0,0.0,0.0,0.0,0.0 +0,0.898300,0.0,0.0,0.0,0.0,0.0 +0,0.898400,0.0,0.0,0.0,0.0,0.0 +0,0.898500,0.0,0.0,0.0,0.0,0.0 +0,0.898600,0.0,0.0,0.0,0.0,0.0 +0,0.898700,0.0,0.0,0.0,0.0,0.0 +0,0.898800,0.0,0.0,0.0,0.0,0.0 +0,0.898900,0.0,0.0,0.0,0.0,0.0 +0,0.899000,0.0,0.0,0.0,0.0,0.0 +0,0.899100,0.0,0.0,0.0,0.0,0.0 +0,0.899200,0.0,0.0,0.0,0.0,0.0 +0,0.899300,0.0,0.0,0.0,0.0,0.0 +0,0.899400,0.0,0.0,0.0,0.0,0.0 +0,0.899500,0.0,0.0,0.0,0.0,0.0 +0,0.899600,0.0,0.0,0.0,0.0,0.0 +0,0.899700,0.0,0.0,0.0,0.0,0.0 +0,0.899800,0.0,0.0,0.0,0.0,0.0 +0,0.899900,0.0,0.0,0.0,0.0,0.0 +0,0.900000,0.0,0.0,0.0,0.0,0.0 +0,0.900100,0.0,0.0,0.0,0.0,0.0 +1,303.901899,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.900200,0.0,0.0,0.0,0.0,0.0 +0,0.900300,0.0,0.0,0.0,0.0,0.0 +0,0.900400,0.0,0.0,0.0,0.0,0.0 +0,0.900500,0.0,0.0,0.0,0.0,0.0 +0,0.900600,0.0,0.0,0.0,0.0,0.0 +0,0.900700,0.0,0.0,0.0,0.0,0.0 +0,0.900800,0.0,0.0,0.0,0.0,0.0 +0,0.900900,0.0,0.0,0.0,0.0,0.0 +0,0.901000,0.0,0.0,0.0,0.0,0.0 +0,0.901100,0.0,0.0,0.0,0.0,0.0 +0,0.901200,0.0,0.0,0.0,0.0,0.0 +0,0.901300,0.0,0.0,0.0,0.0,0.0 +0,0.901400,0.0,0.0,0.0,0.0,0.0 +0,0.901500,0.0,0.0,0.0,0.0,0.0 +0,0.901600,0.0,0.0,0.0,0.0,0.0 +0,0.901700,0.0,0.0,0.0,0.0,0.0 +0,0.901800,0.0,0.0,0.0,0.0,0.0 +0,0.901900,0.0,0.0,0.0,0.0,0.0 +0,0.902000,0.0,0.0,0.0,0.0,0.0 +0,0.902100,0.0,0.0,0.0,0.0,0.0 +0,0.902200,0.0,0.0,0.0,0.0,0.0 +0,0.902300,0.0,0.0,0.0,0.0,0.0 +0,0.902400,0.0,0.0,0.0,0.0,0.0 +0,0.902500,0.0,0.0,0.0,0.0,0.0 +0,0.902600,0.0,0.0,0.0,0.0,0.0 +0,0.902700,0.0,0.0,0.0,0.0,0.0 +0,0.902800,0.0,0.0,0.0,0.0,0.0 +0,0.902900,0.0,0.0,0.0,0.0,0.0 +0,0.903000,0.0,0.0,0.0,0.0,0.0 +0,0.903100,0.0,0.0,0.0,0.0,0.0 +0,0.903200,0.0,0.0,0.0,0.0,0.0 +0,0.903300,0.0,0.0,0.0,0.0,0.0 +0,0.903400,0.0,0.0,0.0,0.0,0.0 +0,0.903500,0.0,0.0,0.0,0.0,0.0 +0,0.903600,0.0,0.0,0.0,0.0,0.0 +0,0.903700,0.0,0.0,0.0,0.0,0.0 +0,0.903800,0.0,0.0,0.0,0.0,0.0 +0,0.903900,0.0,0.0,0.0,0.0,0.0 +0,0.904000,0.0,0.0,0.0,0.0,0.0 +0,0.904100,0.0,0.0,0.0,0.0,0.0 +0,0.904200,0.0,0.0,0.0,0.0,0.0 +0,0.904300,0.0,0.0,0.0,0.0,0.0 +0,0.904400,0.0,0.0,0.0,0.0,0.0 +0,0.904500,0.0,0.0,0.0,0.0,0.0 +0,0.904600,0.0,0.0,0.0,0.0,0.0 +0,0.904700,0.0,0.0,0.0,0.0,0.0 +0,0.904800,0.0,0.0,0.0,0.0,0.0 +0,0.904900,0.0,0.0,0.0,0.0,0.0 +0,0.905000,0.0,0.0,0.0,0.0,0.0 +0,0.905100,0.0,0.0,0.0,0.0,0.0 +0,0.905200,0.0,0.0,0.0,0.0,0.0 +0,0.905300,0.0,0.0,0.0,0.0,0.0 +0,0.905400,0.0,0.0,0.0,0.0,0.0 +0,0.905500,0.0,0.0,0.0,0.0,0.0 +0,0.905600,0.0,0.0,0.0,0.0,0.0 +0,0.905700,0.0,0.0,0.0,0.0,0.0 +0,0.905800,0.0,0.0,0.0,0.0,0.0 +0,0.905900,0.0,0.0,0.0,0.0,0.0 +0,0.906000,0.0,0.0,0.0,0.0,0.0 +0,0.906100,0.0,0.0,0.0,0.0,0.0 +0,0.906200,0.0,0.0,0.0,0.0,0.0 +0,0.906300,0.0,0.0,0.0,0.0,0.0 +0,0.906400,0.0,0.0,0.0,0.0,0.0 +0,0.906500,0.0,0.0,0.0,0.0,0.0 +0,0.906600,0.0,0.0,0.0,0.0,0.0 +0,0.906700,0.0,0.0,0.0,0.0,0.0 +0,0.906800,0.0,0.0,0.0,0.0,0.0 +0,0.906900,0.0,0.0,0.0,0.0,0.0 +0,0.907000,0.0,0.0,0.0,0.0,0.0 +0,0.907100,0.0,0.0,0.0,0.0,0.0 +0,0.907200,0.0,0.0,0.0,0.0,0.0 +0,0.907300,0.0,0.0,0.0,0.0,0.0 +0,0.907400,0.0,0.0,0.0,0.0,0.0 +0,0.907500,0.0,0.0,0.0,0.0,0.0 +0,0.907600,0.0,0.0,0.0,0.0,0.0 +0,0.907700,0.0,0.0,0.0,0.0,0.0 +0,0.907800,0.0,0.0,0.0,0.0,0.0 +0,0.907900,0.0,0.0,0.0,0.0,0.0 +0,0.908000,0.0,0.0,0.0,0.0,0.0 +0,0.908100,0.0,0.0,0.0,0.0,0.0 +0,0.908200,0.0,0.0,0.0,0.0,0.0 +0,0.908300,0.0,0.0,0.0,0.0,0.0 +0,0.908400,0.0,0.0,0.0,0.0,0.0 +0,0.908500,0.0,0.0,0.0,0.0,0.0 +0,0.908600,0.0,0.0,0.0,0.0,0.0 +0,0.908700,0.0,0.0,0.0,0.0,0.0 +0,0.908800,0.0,0.0,0.0,0.0,0.0 +0,0.908900,0.0,0.0,0.0,0.0,0.0 +0,0.909000,0.0,0.0,0.0,0.0,0.0 +0,0.909100,0.0,0.0,0.0,0.0,0.0 +0,0.909200,0.0,0.0,0.0,0.0,0.0 +0,0.909300,0.0,0.0,0.0,0.0,0.0 +0,0.909400,0.0,0.0,0.0,0.0,0.0 +0,0.909500,0.0,0.0,0.0,0.0,0.0 +0,0.909600,0.0,0.0,0.0,0.0,0.0 +0,0.909700,0.0,0.0,0.0,0.0,0.0 +0,0.909800,0.0,0.0,0.0,0.0,0.0 +0,0.909900,0.0,0.0,0.0,0.0,0.0 +0,0.910000,0.0,0.0,0.0,0.0,0.0 +0,0.910100,0.0,0.0,0.0,0.0,0.0 +0,0.910200,0.0,0.0,0.0,0.0,0.0 +0,0.910300,0.0,0.0,0.0,0.0,0.0 +0,0.910400,0.0,0.0,0.0,0.0,0.0 +0,0.910500,0.0,0.0,0.0,0.0,0.0 +0,0.910600,0.0,0.0,0.0,0.0,0.0 +0,0.910700,0.0,0.0,0.0,0.0,0.0 +0,0.910800,0.0,0.0,0.0,0.0,0.0 +0,0.910900,0.0,0.0,0.0,0.0,0.0 +0,0.911000,0.0,0.0,0.0,0.0,0.0 +0,0.911100,0.0,0.0,0.0,0.0,0.0 +0,0.911200,0.0,0.0,0.0,0.0,0.0 +0,0.911300,0.0,0.0,0.0,0.0,0.0 +0,0.911400,0.0,0.0,0.0,0.0,0.0 +0,0.911500,0.0,0.0,0.0,0.0,0.0 +0,0.911600,0.0,0.0,0.0,0.0,0.0 +0,0.911700,0.0,0.0,0.0,0.0,0.0 +0,0.911800,0.0,0.0,0.0,0.0,0.0 +0,0.911900,0.0,0.0,0.0,0.0,0.0 +0,0.912000,0.0,0.0,0.0,0.0,0.0 +0,0.912100,0.0,0.0,0.0,0.0,0.0 +0,0.912200,0.0,0.0,0.0,0.0,0.0 +0,0.912300,0.0,0.0,0.0,0.0,0.0 +0,0.912400,0.0,0.0,0.0,0.0,0.0 +0,0.912500,0.0,0.0,0.0,0.0,0.0 +0,0.912600,0.0,0.0,0.0,0.0,0.0 +0,0.912700,0.0,0.0,0.0,0.0,0.0 +0,0.912800,0.0,0.0,0.0,0.0,0.0 +0,0.912900,0.0,0.0,0.0,0.0,0.0 +0,0.913000,0.0,0.0,0.0,0.0,0.0 +0,0.913100,0.0,0.0,0.0,0.0,0.0 +0,0.913200,0.0,0.0,0.0,0.0,0.0 +0,0.913300,0.0,0.0,0.0,0.0,0.0 +0,0.913400,0.0,0.0,0.0,0.0,0.0 +0,0.913500,0.0,0.0,0.0,0.0,0.0 +0,0.913600,0.0,0.0,0.0,0.0,0.0 +0,0.913700,0.0,0.0,0.0,0.0,0.0 +0,0.913800,0.0,0.0,0.0,0.0,0.0 +0,0.913900,0.0,0.0,0.0,0.0,0.0 +0,0.914000,0.0,0.0,0.0,0.0,0.0 +0,0.914100,0.0,0.0,0.0,0.0,0.0 +0,0.914200,0.0,0.0,0.0,0.0,0.0 +0,0.914300,0.0,0.0,0.0,0.0,0.0 +0,0.914400,0.0,0.0,0.0,0.0,0.0 +0,0.914500,0.0,0.0,0.0,0.0,0.0 +0,0.914600,0.0,0.0,0.0,0.0,0.0 +0,0.914700,0.0,0.0,0.0,0.0,0.0 +0,0.914800,0.0,0.0,0.0,0.0,0.0 +0,0.914900,0.0,0.0,0.0,0.0,0.0 +0,0.915000,0.0,0.0,0.0,0.0,0.0 +0,0.915100,0.0,0.0,0.0,0.0,0.0 +0,0.915200,0.0,0.0,0.0,0.0,0.0 +0,0.915300,0.0,0.0,0.0,0.0,0.0 +0,0.915400,0.0,0.0,0.0,0.0,0.0 +0,0.915500,0.0,0.0,0.0,0.0,0.0 +0,0.915600,0.0,0.0,0.0,0.0,0.0 +0,0.915700,0.0,0.0,0.0,0.0,0.0 +0,0.915800,0.0,0.0,0.0,0.0,0.0 +0,0.915900,0.0,0.0,0.0,0.0,0.0 +0,0.916000,0.0,0.0,0.0,0.0,0.0 +0,0.916100,0.0,0.0,0.0,0.0,0.0 +0,0.916200,0.0,0.0,0.0,0.0,0.0 +0,0.916300,0.0,0.0,0.0,0.0,0.0 +0,0.916400,0.0,0.0,0.0,0.0,0.0 +0,0.916500,0.0,0.0,0.0,0.0,0.0 +0,0.916600,0.0,0.0,0.0,0.0,0.0 +0,0.916700,0.0,0.0,0.0,0.0,0.0 +0,0.916800,0.0,0.0,0.0,0.0,0.0 +0,0.916900,0.0,0.0,0.0,0.0,0.0 +0,0.917000,0.0,0.0,0.0,0.0,0.0 +0,0.917100,0.0,0.0,0.0,0.0,0.0 +0,0.917200,0.0,0.0,0.0,0.0,0.0 +0,0.917300,0.0,0.0,0.0,0.0,0.0 +0,0.917400,0.0,0.0,0.0,0.0,0.0 +0,0.917500,0.0,0.0,0.0,0.0,0.0 +0,0.917600,0.0,0.0,0.0,0.0,0.0 +0,0.917700,0.0,0.0,0.0,0.0,0.0 +0,0.917800,0.0,0.0,0.0,0.0,0.0 +0,0.917900,0.0,0.0,0.0,0.0,0.0 +0,0.918000,0.0,0.0,0.0,0.0,0.0 +0,0.918100,0.0,0.0,0.0,0.0,0.0 +0,0.918200,0.0,0.0,0.0,0.0,0.0 +0,0.918300,0.0,0.0,0.0,0.0,0.0 +0,0.918400,0.0,0.0,0.0,0.0,0.0 +0,0.918500,0.0,0.0,0.0,0.0,0.0 +0,0.918600,0.0,0.0,0.0,0.0,0.0 +0,0.918700,0.0,0.0,0.0,0.0,0.0 +0,0.918800,0.0,0.0,0.0,0.0,0.0 +0,0.918900,0.0,0.0,0.0,0.0,0.0 +0,0.919000,0.0,0.0,0.0,0.0,0.0 +0,0.919100,0.0,0.0,0.0,0.0,0.0 +0,0.919200,0.0,0.0,0.0,0.0,0.0 +0,0.919300,0.0,0.0,0.0,0.0,0.0 +0,0.919400,0.0,0.0,0.0,0.0,0.0 +0,0.919500,0.0,0.0,0.0,0.0,0.0 +0,0.919600,0.0,0.0,0.0,0.0,0.0 +0,0.919700,0.0,0.0,0.0,0.0,0.0 +0,0.919800,0.0,0.0,0.0,0.0,0.0 +0,0.919900,0.0,0.0,0.0,0.0,0.0 +0,0.920000,0.0,0.0,0.0,0.0,0.0 +0,0.920100,0.0,0.0,0.0,0.0,0.0 +1,324.612058,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.920200,0.0,0.0,0.0,0.0,0.0 +0,0.920300,0.0,0.0,0.0,0.0,0.0 +0,0.920400,0.0,0.0,0.0,0.0,0.0 +0,0.920500,0.0,0.0,0.0,0.0,0.0 +0,0.920600,0.0,0.0,0.0,0.0,0.0 +0,0.920700,0.0,0.0,0.0,0.0,0.0 +0,0.920800,0.0,0.0,0.0,0.0,0.0 +0,0.920900,0.0,0.0,0.0,0.0,0.0 +0,0.921000,0.0,0.0,0.0,0.0,0.0 +0,0.921100,0.0,0.0,0.0,0.0,0.0 +0,0.921200,0.0,0.0,0.0,0.0,0.0 +0,0.921300,0.0,0.0,0.0,0.0,0.0 +0,0.921400,0.0,0.0,0.0,0.0,0.0 +0,0.921500,0.0,0.0,0.0,0.0,0.0 +0,0.921600,0.0,0.0,0.0,0.0,0.0 +0,0.921700,0.0,0.0,0.0,0.0,0.0 +0,0.921800,0.0,0.0,0.0,0.0,0.0 +0,0.921900,0.0,0.0,0.0,0.0,0.0 +0,0.922000,0.0,0.0,0.0,0.0,0.0 +0,0.922100,0.0,0.0,0.0,0.0,0.0 +0,0.922200,0.0,0.0,0.0,0.0,0.0 +0,0.922300,0.0,0.0,0.0,0.0,0.0 +0,0.922400,0.0,0.0,0.0,0.0,0.0 +0,0.922500,0.0,0.0,0.0,0.0,0.0 +0,0.922600,0.0,0.0,0.0,0.0,0.0 +0,0.922700,0.0,0.0,0.0,0.0,0.0 +0,0.922800,0.0,0.0,0.0,0.0,0.0 +0,0.922900,0.0,0.0,0.0,0.0,0.0 +0,0.923000,0.0,0.0,0.0,0.0,0.0 +0,0.923100,0.0,0.0,0.0,0.0,0.0 +0,0.923200,0.0,0.0,0.0,0.0,0.0 +0,0.923300,0.0,0.0,0.0,0.0,0.0 +0,0.923400,0.0,0.0,0.0,0.0,0.0 +0,0.923500,0.0,0.0,0.0,0.0,0.0 +0,0.923600,0.0,0.0,0.0,0.0,0.0 +0,0.923700,0.0,0.0,0.0,0.0,0.0 +0,0.923800,0.0,0.0,0.0,0.0,0.0 +0,0.923900,0.0,0.0,0.0,0.0,0.0 +0,0.924000,0.0,0.0,0.0,0.0,0.0 +0,0.924100,0.0,0.0,0.0,0.0,0.0 +0,0.924200,0.0,0.0,0.0,0.0,0.0 +0,0.924300,0.0,0.0,0.0,0.0,0.0 +0,0.924400,0.0,0.0,0.0,0.0,0.0 +0,0.924500,0.0,0.0,0.0,0.0,0.0 +0,0.924600,0.0,0.0,0.0,0.0,0.0 +0,0.924700,0.0,0.0,0.0,0.0,0.0 +0,0.924800,0.0,0.0,0.0,0.0,0.0 +0,0.924900,0.0,0.0,0.0,0.0,0.0 +0,0.925000,0.0,0.0,0.0,0.0,0.0 +0,0.925100,0.0,0.0,0.0,0.0,0.0 +0,0.925200,0.0,0.0,0.0,0.0,0.0 +0,0.925300,0.0,0.0,0.0,0.0,0.0 +0,0.925400,0.0,0.0,0.0,0.0,0.0 +0,0.925500,0.0,0.0,0.0,0.0,0.0 +0,0.925600,0.0,0.0,0.0,0.0,0.0 +0,0.925700,0.0,0.0,0.0,0.0,0.0 +0,0.925800,0.0,0.0,0.0,0.0,0.0 +0,0.925900,0.0,0.0,0.0,0.0,0.0 +0,0.926000,0.0,0.0,0.0,0.0,0.0 +0,0.926100,0.0,0.0,0.0,0.0,0.0 +0,0.926200,0.0,0.0,0.0,0.0,0.0 +0,0.926300,0.0,0.0,0.0,0.0,0.0 +0,0.926400,0.0,0.0,0.0,0.0,0.0 +0,0.926500,0.0,0.0,0.0,0.0,0.0 +0,0.926600,0.0,0.0,0.0,0.0,0.0 +0,0.926700,0.0,0.0,0.0,0.0,0.0 +0,0.926800,0.0,0.0,0.0,0.0,0.0 +0,0.926900,0.0,0.0,0.0,0.0,0.0 +0,0.927000,0.0,0.0,0.0,0.0,0.0 +0,0.927100,0.0,0.0,0.0,0.0,0.0 +0,0.927200,0.0,0.0,0.0,0.0,0.0 +0,0.927300,0.0,0.0,0.0,0.0,0.0 +0,0.927400,0.0,0.0,0.0,0.0,0.0 +0,0.927500,0.0,0.0,0.0,0.0,0.0 +0,0.927600,0.0,0.0,0.0,0.0,0.0 +0,0.927700,0.0,0.0,0.0,0.0,0.0 +0,0.927800,0.0,0.0,0.0,0.0,0.0 +0,0.927900,0.0,0.0,0.0,0.0,0.0 +0,0.928000,0.0,0.0,0.0,0.0,0.0 +0,0.928100,0.0,0.0,0.0,0.0,0.0 +0,0.928200,0.0,0.0,0.0,0.0,0.0 +0,0.928300,0.0,0.0,0.0,0.0,0.0 +0,0.928400,0.0,0.0,0.0,0.0,0.0 +0,0.928500,0.0,0.0,0.0,0.0,0.0 +0,0.928600,0.0,0.0,0.0,0.0,0.0 +0,0.928700,0.0,0.0,0.0,0.0,0.0 +0,0.928800,0.0,0.0,0.0,0.0,0.0 +0,0.928900,0.0,0.0,0.0,0.0,0.0 +0,0.929000,0.0,0.0,0.0,0.0,0.0 +0,0.929100,0.0,0.0,0.0,0.0,0.0 +0,0.929200,0.0,0.0,0.0,0.0,0.0 +0,0.929300,0.0,0.0,0.0,0.0,0.0 +0,0.929400,0.0,0.0,0.0,0.0,0.0 +0,0.929500,0.0,0.0,0.0,0.0,0.0 +0,0.929600,0.0,0.0,0.0,0.0,0.0 +0,0.929700,0.0,0.0,0.0,0.0,0.0 +0,0.929800,0.0,0.0,0.0,0.0,0.0 +0,0.929900,0.0,0.0,0.0,0.0,0.0 +0,0.930000,0.0,0.0,0.0,0.0,0.0 +0,0.930100,0.0,0.0,0.0,0.0,0.0 +0,0.930200,0.0,0.0,0.0,0.0,0.0 +0,0.930300,0.0,0.0,0.0,0.0,0.0 +0,0.930400,0.0,0.0,0.0,0.0,0.0 +0,0.930500,0.0,0.0,0.0,0.0,0.0 +0,0.930600,0.0,0.0,0.0,0.0,0.0 +0,0.930700,0.0,0.0,0.0,0.0,0.0 +0,0.930800,0.0,0.0,0.0,0.0,0.0 +0,0.930900,0.0,0.0,0.0,0.0,0.0 +0,0.931000,0.0,0.0,0.0,0.0,0.0 +0,0.931100,0.0,0.0,0.0,0.0,0.0 +0,0.931200,0.0,0.0,0.0,0.0,0.0 +0,0.931300,0.0,0.0,0.0,0.0,0.0 +0,0.931400,0.0,0.0,0.0,0.0,0.0 +0,0.931500,0.0,0.0,0.0,0.0,0.0 +0,0.931600,0.0,0.0,0.0,0.0,0.0 +0,0.931700,0.0,0.0,0.0,0.0,0.0 +0,0.931800,0.0,0.0,0.0,0.0,0.0 +0,0.931900,0.0,0.0,0.0,0.0,0.0 +0,0.932000,0.0,0.0,0.0,0.0,0.0 +0,0.932100,0.0,0.0,0.0,0.0,0.0 +0,0.932200,0.0,0.0,0.0,0.0,0.0 +0,0.932300,0.0,0.0,0.0,0.0,0.0 +0,0.932400,0.0,0.0,0.0,0.0,0.0 +0,0.932500,0.0,0.0,0.0,0.0,0.0 +0,0.932600,0.0,0.0,0.0,0.0,0.0 +0,0.932700,0.0,0.0,0.0,0.0,0.0 +0,0.932800,0.0,0.0,0.0,0.0,0.0 +0,0.932900,0.0,0.0,0.0,0.0,0.0 +0,0.933000,0.0,0.0,0.0,0.0,0.0 +0,0.933100,0.0,0.0,0.0,0.0,0.0 +0,0.933200,0.0,0.0,0.0,0.0,0.0 +0,0.933300,0.0,0.0,0.0,0.0,0.0 +0,0.933400,0.0,0.0,0.0,0.0,0.0 +0,0.933500,0.0,0.0,0.0,0.0,0.0 +0,0.933600,0.0,0.0,0.0,0.0,0.0 +0,0.933700,0.0,0.0,0.0,0.0,0.0 +0,0.933800,0.0,0.0,0.0,0.0,0.0 +0,0.933900,0.0,0.0,0.0,0.0,0.0 +0,0.934000,0.0,0.0,0.0,0.0,0.0 +0,0.934100,0.0,0.0,0.0,0.0,0.0 +0,0.934200,0.0,0.0,0.0,0.0,0.0 +0,0.934300,0.0,0.0,0.0,0.0,0.0 +0,0.934400,0.0,0.0,0.0,0.0,0.0 +0,0.934500,0.0,0.0,0.0,0.0,0.0 +0,0.934600,0.0,0.0,0.0,0.0,0.0 +0,0.934700,0.0,0.0,0.0,0.0,0.0 +0,0.934800,0.0,0.0,0.0,0.0,0.0 +0,0.934900,0.0,0.0,0.0,0.0,0.0 +0,0.935000,0.0,0.0,0.0,0.0,0.0 +0,0.935100,0.0,0.0,0.0,0.0,0.0 +0,0.935200,0.0,0.0,0.0,0.0,0.0 +0,0.935300,0.0,0.0,0.0,0.0,0.0 +0,0.935400,0.0,0.0,0.0,0.0,0.0 +0,0.935500,0.0,0.0,0.0,0.0,0.0 +0,0.935600,0.0,0.0,0.0,0.0,0.0 +0,0.935700,0.0,0.0,0.0,0.0,0.0 +0,0.935800,0.0,0.0,0.0,0.0,0.0 +0,0.935900,0.0,0.0,0.0,0.0,0.0 +0,0.936000,0.0,0.0,0.0,0.0,0.0 +0,0.936100,0.0,0.0,0.0,0.0,0.0 +0,0.936200,0.0,0.0,0.0,0.0,0.0 +0,0.936300,0.0,0.0,0.0,0.0,0.0 +0,0.936400,0.0,0.0,0.0,0.0,0.0 +0,0.936500,0.0,0.0,0.0,0.0,0.0 +0,0.936600,0.0,0.0,0.0,0.0,0.0 +0,0.936700,0.0,0.0,0.0,0.0,0.0 +0,0.936800,0.0,0.0,0.0,0.0,0.0 +0,0.936900,0.0,0.0,0.0,0.0,0.0 +0,0.937000,0.0,0.0,0.0,0.0,0.0 +0,0.937100,0.0,0.0,0.0,0.0,0.0 +0,0.937200,0.0,0.0,0.0,0.0,0.0 +0,0.937300,0.0,0.0,0.0,0.0,0.0 +0,0.937400,0.0,0.0,0.0,0.0,0.0 +0,0.937500,0.0,0.0,0.0,0.0,0.0 +0,0.937600,0.0,0.0,0.0,0.0,0.0 +0,0.937700,0.0,0.0,0.0,0.0,0.0 +0,0.937800,0.0,0.0,0.0,0.0,0.0 +0,0.937900,0.0,0.0,0.0,0.0,0.0 +0,0.938000,0.0,0.0,0.0,0.0,0.0 +0,0.938100,0.0,0.0,0.0,0.0,0.0 +0,0.938200,0.0,0.0,0.0,0.0,0.0 +0,0.938300,0.0,0.0,0.0,0.0,0.0 +0,0.938400,0.0,0.0,0.0,0.0,0.0 +0,0.938500,0.0,0.0,0.0,0.0,0.0 +0,0.938600,0.0,0.0,0.0,0.0,0.0 +0,0.938700,0.0,0.0,0.0,0.0,0.0 +0,0.938800,0.0,0.0,0.0,0.0,0.0 +0,0.938900,0.0,0.0,0.0,0.0,0.0 +0,0.939000,0.0,0.0,0.0,0.0,0.0 +0,0.939100,0.0,0.0,0.0,0.0,0.0 +0,0.939200,0.0,0.0,0.0,0.0,0.0 +0,0.939300,0.0,0.0,0.0,0.0,0.0 +0,0.939400,0.0,0.0,0.0,0.0,0.0 +0,0.939500,0.0,0.0,0.0,0.0,0.0 +0,0.939600,0.0,0.0,0.0,0.0,0.0 +0,0.939700,0.0,0.0,0.0,0.0,0.0 +0,0.939800,0.0,0.0,0.0,0.0,0.0 +0,0.939900,0.0,0.0,0.0,0.0,0.0 +0,0.940000,0.0,0.0,0.0,0.0,0.0 +0,0.940100,0.0,0.0,0.0,0.0,0.0 +1,346.242367,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.940200,0.0,0.0,0.0,0.0,0.0 +0,0.940300,0.0,0.0,0.0,0.0,0.0 +0,0.940400,0.0,0.0,0.0,0.0,0.0 +0,0.940500,0.0,0.0,0.0,0.0,0.0 +0,0.940600,0.0,0.0,0.0,0.0,0.0 +0,0.940700,0.0,0.0,0.0,0.0,0.0 +0,0.940800,0.0,0.0,0.0,0.0,0.0 +0,0.940900,0.0,0.0,0.0,0.0,0.0 +0,0.941000,0.0,0.0,0.0,0.0,0.0 +0,0.941100,0.0,0.0,0.0,0.0,0.0 +0,0.941200,0.0,0.0,0.0,0.0,0.0 +0,0.941300,0.0,0.0,0.0,0.0,0.0 +0,0.941400,0.0,0.0,0.0,0.0,0.0 +0,0.941500,0.0,0.0,0.0,0.0,0.0 +0,0.941600,0.0,0.0,0.0,0.0,0.0 +0,0.941700,0.0,0.0,0.0,0.0,0.0 +0,0.941800,0.0,0.0,0.0,0.0,0.0 +0,0.941900,0.0,0.0,0.0,0.0,0.0 +0,0.942000,0.0,0.0,0.0,0.0,0.0 +0,0.942100,0.0,0.0,0.0,0.0,0.0 +0,0.942200,0.0,0.0,0.0,0.0,0.0 +0,0.942300,0.0,0.0,0.0,0.0,0.0 +0,0.942400,0.0,0.0,0.0,0.0,0.0 +0,0.942500,0.0,0.0,0.0,0.0,0.0 +0,0.942600,0.0,0.0,0.0,0.0,0.0 +0,0.942700,0.0,0.0,0.0,0.0,0.0 +0,0.942800,0.0,0.0,0.0,0.0,0.0 +0,0.942900,0.0,0.0,0.0,0.0,0.0 +0,0.943000,0.0,0.0,0.0,0.0,0.0 +0,0.943100,0.0,0.0,0.0,0.0,0.0 +0,0.943200,0.0,0.0,0.0,0.0,0.0 +0,0.943300,0.0,0.0,0.0,0.0,0.0 +0,0.943400,0.0,0.0,0.0,0.0,0.0 +0,0.943500,0.0,0.0,0.0,0.0,0.0 +0,0.943600,0.0,0.0,0.0,0.0,0.0 +0,0.943700,0.0,0.0,0.0,0.0,0.0 +0,0.943800,0.0,0.0,0.0,0.0,0.0 +0,0.943900,0.0,0.0,0.0,0.0,0.0 +0,0.944000,0.0,0.0,0.0,0.0,0.0 +0,0.944100,0.0,0.0,0.0,0.0,0.0 +0,0.944200,0.0,0.0,0.0,0.0,0.0 +0,0.944300,0.0,0.0,0.0,0.0,0.0 +0,0.944400,0.0,0.0,0.0,0.0,0.0 +0,0.944500,0.0,0.0,0.0,0.0,0.0 +0,0.944600,0.0,0.0,0.0,0.0,0.0 +0,0.944700,0.0,0.0,0.0,0.0,0.0 +0,0.944800,0.0,0.0,0.0,0.0,0.0 +0,0.944900,0.0,0.0,0.0,0.0,0.0 +0,0.945000,0.0,0.0,0.0,0.0,0.0 +0,0.945100,0.0,0.0,0.0,0.0,0.0 +0,0.945200,0.0,0.0,0.0,0.0,0.0 +0,0.945300,0.0,0.0,0.0,0.0,0.0 +0,0.945400,0.0,0.0,0.0,0.0,0.0 +0,0.945500,0.0,0.0,0.0,0.0,0.0 +0,0.945600,0.0,0.0,0.0,0.0,0.0 +0,0.945700,0.0,0.0,0.0,0.0,0.0 +0,0.945800,0.0,0.0,0.0,0.0,0.0 +0,0.945900,0.0,0.0,0.0,0.0,0.0 +0,0.946000,0.0,0.0,0.0,0.0,0.0 +0,0.946100,0.0,0.0,0.0,0.0,0.0 +0,0.946200,0.0,0.0,0.0,0.0,0.0 +0,0.946300,0.0,0.0,0.0,0.0,0.0 +0,0.946400,0.0,0.0,0.0,0.0,0.0 +0,0.946500,0.0,0.0,0.0,0.0,0.0 +0,0.946600,0.0,0.0,0.0,0.0,0.0 +0,0.946700,0.0,0.0,0.0,0.0,0.0 +0,0.946800,0.0,0.0,0.0,0.0,0.0 +0,0.946900,0.0,0.0,0.0,0.0,0.0 +0,0.947000,0.0,0.0,0.0,0.0,0.0 +0,0.947100,0.0,0.0,0.0,0.0,0.0 +0,0.947200,0.0,0.0,0.0,0.0,0.0 +0,0.947300,0.0,0.0,0.0,0.0,0.0 +0,0.947400,0.0,0.0,0.0,0.0,0.0 +0,0.947500,0.0,0.0,0.0,0.0,0.0 +0,0.947600,0.0,0.0,0.0,0.0,0.0 +0,0.947700,0.0,0.0,0.0,0.0,0.0 +0,0.947800,0.0,0.0,0.0,0.0,0.0 +0,0.947900,0.0,0.0,0.0,0.0,0.0 +0,0.948000,0.0,0.0,0.0,0.0,0.0 +0,0.948100,0.0,0.0,0.0,0.0,0.0 +0,0.948200,0.0,0.0,0.0,0.0,0.0 +0,0.948300,0.0,0.0,0.0,0.0,0.0 +0,0.948400,0.0,0.0,0.0,0.0,0.0 +0,0.948500,0.0,0.0,0.0,0.0,0.0 +0,0.948600,0.0,0.0,0.0,0.0,0.0 +0,0.948700,0.0,0.0,0.0,0.0,0.0 +0,0.948800,0.0,0.0,0.0,0.0,0.0 +0,0.948900,0.0,0.0,0.0,0.0,0.0 +0,0.949000,0.0,0.0,0.0,0.0,0.0 +0,0.949100,0.0,0.0,0.0,0.0,0.0 +0,0.949200,0.0,0.0,0.0,0.0,0.0 +0,0.949300,0.0,0.0,0.0,0.0,0.0 +0,0.949400,0.0,0.0,0.0,0.0,0.0 +0,0.949500,0.0,0.0,0.0,0.0,0.0 +0,0.949600,0.0,0.0,0.0,0.0,0.0 +0,0.949700,0.0,0.0,0.0,0.0,0.0 +0,0.949800,0.0,0.0,0.0,0.0,0.0 +0,0.949900,0.0,0.0,0.0,0.0,0.0 +0,0.950000,0.0,0.0,0.0,0.0,0.0 +0,0.950100,0.0,0.0,0.0,0.0,0.0 +0,0.950200,0.0,0.0,0.0,0.0,0.0 +0,0.950300,0.0,0.0,0.0,0.0,0.0 +0,0.950400,0.0,0.0,0.0,0.0,0.0 +0,0.950500,0.0,0.0,0.0,0.0,0.0 +0,0.950600,0.0,0.0,0.0,0.0,0.0 +0,0.950700,0.0,0.0,0.0,0.0,0.0 +0,0.950800,0.0,0.0,0.0,0.0,0.0 +0,0.950900,0.0,0.0,0.0,0.0,0.0 +0,0.951000,0.0,0.0,0.0,0.0,0.0 +0,0.951100,0.0,0.0,0.0,0.0,0.0 +0,0.951200,0.0,0.0,0.0,0.0,0.0 +0,0.951300,0.0,0.0,0.0,0.0,0.0 +0,0.951400,0.0,0.0,0.0,0.0,0.0 +0,0.951500,0.0,0.0,0.0,0.0,0.0 +0,0.951600,0.0,0.0,0.0,0.0,0.0 +0,0.951700,0.0,0.0,0.0,0.0,0.0 +0,0.951800,0.0,0.0,0.0,0.0,0.0 +0,0.951900,0.0,0.0,0.0,0.0,0.0 +0,0.952000,0.0,0.0,0.0,0.0,0.0 +0,0.952100,0.0,0.0,0.0,0.0,0.0 +0,0.952200,0.0,0.0,0.0,0.0,0.0 +0,0.952300,0.0,0.0,0.0,0.0,0.0 +0,0.952400,0.0,0.0,0.0,0.0,0.0 +0,0.952500,0.0,0.0,0.0,0.0,0.0 +0,0.952600,0.0,0.0,0.0,0.0,0.0 +0,0.952700,0.0,0.0,0.0,0.0,0.0 +0,0.952800,0.0,0.0,0.0,0.0,0.0 +0,0.952900,0.0,0.0,0.0,0.0,0.0 +0,0.953000,0.0,0.0,0.0,0.0,0.0 +0,0.953100,0.0,0.0,0.0,0.0,0.0 +0,0.953200,0.0,0.0,0.0,0.0,0.0 +0,0.953300,0.0,0.0,0.0,0.0,0.0 +0,0.953400,0.0,0.0,0.0,0.0,0.0 +0,0.953500,0.0,0.0,0.0,0.0,0.0 +0,0.953600,0.0,0.0,0.0,0.0,0.0 +0,0.953700,0.0,0.0,0.0,0.0,0.0 +0,0.953800,0.0,0.0,0.0,0.0,0.0 +0,0.953900,0.0,0.0,0.0,0.0,0.0 +0,0.954000,0.0,0.0,0.0,0.0,0.0 +0,0.954100,0.0,0.0,0.0,0.0,0.0 +0,0.954200,0.0,0.0,0.0,0.0,0.0 +0,0.954300,0.0,0.0,0.0,0.0,0.0 +0,0.954400,0.0,0.0,0.0,0.0,0.0 +0,0.954500,0.0,0.0,0.0,0.0,0.0 +0,0.954600,0.0,0.0,0.0,0.0,0.0 +0,0.954700,0.0,0.0,0.0,0.0,0.0 +0,0.954800,0.0,0.0,0.0,0.0,0.0 +0,0.954900,0.0,0.0,0.0,0.0,0.0 +0,0.955000,0.0,0.0,0.0,0.0,0.0 +0,0.955100,0.0,0.0,0.0,0.0,0.0 +0,0.955200,0.0,0.0,0.0,0.0,0.0 +0,0.955300,0.0,0.0,0.0,0.0,0.0 +0,0.955400,0.0,0.0,0.0,0.0,0.0 +0,0.955500,0.0,0.0,0.0,0.0,0.0 +0,0.955600,0.0,0.0,0.0,0.0,0.0 +0,0.955700,0.0,0.0,0.0,0.0,0.0 +0,0.955800,0.0,0.0,0.0,0.0,0.0 +0,0.955900,0.0,0.0,0.0,0.0,0.0 +0,0.956000,0.0,0.0,0.0,0.0,0.0 +0,0.956100,0.0,0.0,0.0,0.0,0.0 +0,0.956200,0.0,0.0,0.0,0.0,0.0 +0,0.956300,0.0,0.0,0.0,0.0,0.0 +0,0.956400,0.0,0.0,0.0,0.0,0.0 +0,0.956500,0.0,0.0,0.0,0.0,0.0 +0,0.956600,0.0,0.0,0.0,0.0,0.0 +0,0.956700,0.0,0.0,0.0,0.0,0.0 +0,0.956800,0.0,0.0,0.0,0.0,0.0 +0,0.956900,0.0,0.0,0.0,0.0,0.0 +0,0.957000,0.0,0.0,0.0,0.0,0.0 +0,0.957100,0.0,0.0,0.0,0.0,0.0 +0,0.957200,0.0,0.0,0.0,0.0,0.0 +0,0.957300,0.0,0.0,0.0,0.0,0.0 +0,0.957400,0.0,0.0,0.0,0.0,0.0 +0,0.957500,0.0,0.0,0.0,0.0,0.0 +0,0.957600,0.0,0.0,0.0,0.0,0.0 +0,0.957700,0.0,0.0,0.0,0.0,0.0 +0,0.957800,0.0,0.0,0.0,0.0,0.0 +0,0.957900,0.0,0.0,0.0,0.0,0.0 +0,0.958000,0.0,0.0,0.0,0.0,0.0 +0,0.958100,0.0,0.0,0.0,0.0,0.0 +0,0.958200,0.0,0.0,0.0,0.0,0.0 +0,0.958300,0.0,0.0,0.0,0.0,0.0 +0,0.958400,0.0,0.0,0.0,0.0,0.0 +0,0.958500,0.0,0.0,0.0,0.0,0.0 +0,0.958600,0.0,0.0,0.0,0.0,0.0 +0,0.958700,0.0,0.0,0.0,0.0,0.0 +0,0.958800,0.0,0.0,0.0,0.0,0.0 +0,0.958900,0.0,0.0,0.0,0.0,0.0 +0,0.959000,0.0,0.0,0.0,0.0,0.0 +0,0.959100,0.0,0.0,0.0,0.0,0.0 +0,0.959200,0.0,0.0,0.0,0.0,0.0 +0,0.959300,0.0,0.0,0.0,0.0,0.0 +0,0.959400,0.0,0.0,0.0,0.0,0.0 +0,0.959500,0.0,0.0,0.0,0.0,0.0 +0,0.959600,0.0,0.0,0.0,0.0,0.0 +0,0.959700,0.0,0.0,0.0,0.0,0.0 +0,0.959800,0.0,0.0,0.0,0.0,0.0 +0,0.959900,0.0,0.0,0.0,0.0,0.0 +0,0.960000,0.0,0.0,0.0,0.0,0.0 +0,0.960100,0.0,0.0,0.0,0.0,0.0 +1,368.812826,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.960200,0.0,0.0,0.0,0.0,0.0 +0,0.960300,0.0,0.0,0.0,0.0,0.0 +0,0.960400,0.0,0.0,0.0,0.0,0.0 +0,0.960500,0.0,0.0,0.0,0.0,0.0 +0,0.960600,0.0,0.0,0.0,0.0,0.0 +0,0.960700,0.0,0.0,0.0,0.0,0.0 +0,0.960800,0.0,0.0,0.0,0.0,0.0 +0,0.960900,0.0,0.0,0.0,0.0,0.0 +0,0.961000,0.0,0.0,0.0,0.0,0.0 +0,0.961100,0.0,0.0,0.0,0.0,0.0 +0,0.961200,0.0,0.0,0.0,0.0,0.0 +0,0.961300,0.0,0.0,0.0,0.0,0.0 +0,0.961400,0.0,0.0,0.0,0.0,0.0 +0,0.961500,0.0,0.0,0.0,0.0,0.0 +0,0.961600,0.0,0.0,0.0,0.0,0.0 +0,0.961700,0.0,0.0,0.0,0.0,0.0 +0,0.961800,0.0,0.0,0.0,0.0,0.0 +0,0.961900,0.0,0.0,0.0,0.0,0.0 +0,0.962000,0.0,0.0,0.0,0.0,0.0 +0,0.962100,0.0,0.0,0.0,0.0,0.0 +0,0.962200,0.0,0.0,0.0,0.0,0.0 +0,0.962300,0.0,0.0,0.0,0.0,0.0 +0,0.962400,0.0,0.0,0.0,0.0,0.0 +0,0.962500,0.0,0.0,0.0,0.0,0.0 +0,0.962600,0.0,0.0,0.0,0.0,0.0 +0,0.962700,0.0,0.0,0.0,0.0,0.0 +0,0.962800,0.0,0.0,0.0,0.0,0.0 +0,0.962900,0.0,0.0,0.0,0.0,0.0 +0,0.963000,0.0,0.0,0.0,0.0,0.0 +0,0.963100,0.0,0.0,0.0,0.0,0.0 +0,0.963200,0.0,0.0,0.0,0.0,0.0 +0,0.963300,0.0,0.0,0.0,0.0,0.0 +0,0.963400,0.0,0.0,0.0,0.0,0.0 +0,0.963500,0.0,0.0,0.0,0.0,0.0 +0,0.963600,0.0,0.0,0.0,0.0,0.0 +0,0.963700,0.0,0.0,0.0,0.0,0.0 +0,0.963800,0.0,0.0,0.0,0.0,0.0 +0,0.963900,0.0,0.0,0.0,0.0,0.0 +0,0.964000,0.0,0.0,0.0,0.0,0.0 +0,0.964100,0.0,0.0,0.0,0.0,0.0 +0,0.964200,0.0,0.0,0.0,0.0,0.0 +0,0.964300,0.0,0.0,0.0,0.0,0.0 +0,0.964400,0.0,0.0,0.0,0.0,0.0 +0,0.964500,0.0,0.0,0.0,0.0,0.0 +0,0.964600,0.0,0.0,0.0,0.0,0.0 +0,0.964700,0.0,0.0,0.0,0.0,0.0 +0,0.964800,0.0,0.0,0.0,0.0,0.0 +0,0.964900,0.0,0.0,0.0,0.0,0.0 +0,0.965000,0.0,0.0,0.0,0.0,0.0 +0,0.965100,0.0,0.0,0.0,0.0,0.0 +0,0.965200,0.0,0.0,0.0,0.0,0.0 +0,0.965300,0.0,0.0,0.0,0.0,0.0 +0,0.965400,0.0,0.0,0.0,0.0,0.0 +0,0.965500,0.0,0.0,0.0,0.0,0.0 +0,0.965600,0.0,0.0,0.0,0.0,0.0 +0,0.965700,0.0,0.0,0.0,0.0,0.0 +0,0.965800,0.0,0.0,0.0,0.0,0.0 +0,0.965900,0.0,0.0,0.0,0.0,0.0 +0,0.966000,0.0,0.0,0.0,0.0,0.0 +0,0.966100,0.0,0.0,0.0,0.0,0.0 +0,0.966200,0.0,0.0,0.0,0.0,0.0 +0,0.966300,0.0,0.0,0.0,0.0,0.0 +0,0.966400,0.0,0.0,0.0,0.0,0.0 +0,0.966500,0.0,0.0,0.0,0.0,0.0 +0,0.966600,0.0,0.0,0.0,0.0,0.0 +0,0.966700,0.0,0.0,0.0,0.0,0.0 +0,0.966800,0.0,0.0,0.0,0.0,0.0 +0,0.966900,0.0,0.0,0.0,0.0,0.0 +0,0.967000,0.0,0.0,0.0,0.0,0.0 +0,0.967100,0.0,0.0,0.0,0.0,0.0 +0,0.967200,0.0,0.0,0.0,0.0,0.0 +0,0.967300,0.0,0.0,0.0,0.0,0.0 +0,0.967400,0.0,0.0,0.0,0.0,0.0 +0,0.967500,0.0,0.0,0.0,0.0,0.0 +0,0.967600,0.0,0.0,0.0,0.0,0.0 +0,0.967700,0.0,0.0,0.0,0.0,0.0 +0,0.967800,0.0,0.0,0.0,0.0,0.0 +0,0.967900,0.0,0.0,0.0,0.0,0.0 +0,0.968000,0.0,0.0,0.0,0.0,0.0 +0,0.968100,0.0,0.0,0.0,0.0,0.0 +0,0.968200,0.0,0.0,0.0,0.0,0.0 +0,0.968300,0.0,0.0,0.0,0.0,0.0 +0,0.968400,0.0,0.0,0.0,0.0,0.0 +0,0.968500,0.0,0.0,0.0,0.0,0.0 +0,0.968600,0.0,0.0,0.0,0.0,0.0 +0,0.968700,0.0,0.0,0.0,0.0,0.0 +0,0.968800,0.0,0.0,0.0,0.0,0.0 +0,0.968900,0.0,0.0,0.0,0.0,0.0 +0,0.969000,0.0,0.0,0.0,0.0,0.0 +0,0.969100,0.0,0.0,0.0,0.0,0.0 +0,0.969200,0.0,0.0,0.0,0.0,0.0 +0,0.969300,0.0,0.0,0.0,0.0,0.0 +0,0.969400,0.0,0.0,0.0,0.0,0.0 +0,0.969500,0.0,0.0,0.0,0.0,0.0 +0,0.969600,0.0,0.0,0.0,0.0,0.0 +0,0.969700,0.0,0.0,0.0,0.0,0.0 +0,0.969800,0.0,0.0,0.0,0.0,0.0 +0,0.969900,0.0,0.0,0.0,0.0,0.0 +0,0.970000,0.0,0.0,0.0,0.0,0.0 +0,0.970100,0.0,0.0,0.0,0.0,0.0 +0,0.970200,0.0,0.0,0.0,0.0,0.0 +0,0.970300,0.0,0.0,0.0,0.0,0.0 +0,0.970400,0.0,0.0,0.0,0.0,0.0 +0,0.970500,0.0,0.0,0.0,0.0,0.0 +0,0.970600,0.0,0.0,0.0,0.0,0.0 +0,0.970700,0.0,0.0,0.0,0.0,0.0 +0,0.970800,0.0,0.0,0.0,0.0,0.0 +0,0.970900,0.0,0.0,0.0,0.0,0.0 +0,0.971000,0.0,0.0,0.0,0.0,0.0 +0,0.971100,0.0,0.0,0.0,0.0,0.0 +0,0.971200,0.0,0.0,0.0,0.0,0.0 +0,0.971300,0.0,0.0,0.0,0.0,0.0 +0,0.971400,0.0,0.0,0.0,0.0,0.0 +0,0.971500,0.0,0.0,0.0,0.0,0.0 +0,0.971600,0.0,0.0,0.0,0.0,0.0 +0,0.971700,0.0,0.0,0.0,0.0,0.0 +0,0.971800,0.0,0.0,0.0,0.0,0.0 +0,0.971900,0.0,0.0,0.0,0.0,0.0 +0,0.972000,0.0,0.0,0.0,0.0,0.0 +0,0.972100,0.0,0.0,0.0,0.0,0.0 +0,0.972200,0.0,0.0,0.0,0.0,0.0 +0,0.972300,0.0,0.0,0.0,0.0,0.0 +0,0.972400,0.0,0.0,0.0,0.0,0.0 +0,0.972500,0.0,0.0,0.0,0.0,0.0 +0,0.972600,0.0,0.0,0.0,0.0,0.0 +0,0.972700,0.0,0.0,0.0,0.0,0.0 +0,0.972800,0.0,0.0,0.0,0.0,0.0 +0,0.972900,0.0,0.0,0.0,0.0,0.0 +0,0.973000,0.0,0.0,0.0,0.0,0.0 +0,0.973100,0.0,0.0,0.0,0.0,0.0 +0,0.973200,0.0,0.0,0.0,0.0,0.0 +0,0.973300,0.0,0.0,0.0,0.0,0.0 +0,0.973400,0.0,0.0,0.0,0.0,0.0 +0,0.973500,0.0,0.0,0.0,0.0,0.0 +0,0.973600,0.0,0.0,0.0,0.0,0.0 +0,0.973700,0.0,0.0,0.0,0.0,0.0 +0,0.973800,0.0,0.0,0.0,0.0,0.0 +0,0.973900,0.0,0.0,0.0,0.0,0.0 +0,0.974000,0.0,0.0,0.0,0.0,0.0 +0,0.974100,0.0,0.0,0.0,0.0,0.0 +0,0.974200,0.0,0.0,0.0,0.0,0.0 +0,0.974300,0.0,0.0,0.0,0.0,0.0 +0,0.974400,0.0,0.0,0.0,0.0,0.0 +0,0.974500,0.0,0.0,0.0,0.0,0.0 +0,0.974600,0.0,0.0,0.0,0.0,0.0 +0,0.974700,0.0,0.0,0.0,0.0,0.0 +0,0.974800,0.0,0.0,0.0,0.0,0.0 +0,0.974900,0.0,0.0,0.0,0.0,0.0 +0,0.975000,0.0,0.0,0.0,0.0,0.0 +0,0.975100,0.0,0.0,0.0,0.0,0.0 +0,0.975200,0.0,0.0,0.0,0.0,0.0 +0,0.975300,0.0,0.0,0.0,0.0,0.0 +0,0.975400,0.0,0.0,0.0,0.0,0.0 +0,0.975500,0.0,0.0,0.0,0.0,0.0 +0,0.975600,0.0,0.0,0.0,0.0,0.0 +0,0.975700,0.0,0.0,0.0,0.0,0.0 +0,0.975800,0.0,0.0,0.0,0.0,0.0 +0,0.975900,0.0,0.0,0.0,0.0,0.0 +0,0.976000,0.0,0.0,0.0,0.0,0.0 +0,0.976100,0.0,0.0,0.0,0.0,0.0 +0,0.976200,0.0,0.0,0.0,0.0,0.0 +0,0.976300,0.0,0.0,0.0,0.0,0.0 +0,0.976400,0.0,0.0,0.0,0.0,0.0 +0,0.976500,0.0,0.0,0.0,0.0,0.0 +0,0.976600,0.0,0.0,0.0,0.0,0.0 +0,0.976700,0.0,0.0,0.0,0.0,0.0 +0,0.976800,0.0,0.0,0.0,0.0,0.0 +0,0.976900,0.0,0.0,0.0,0.0,0.0 +0,0.977000,0.0,0.0,0.0,0.0,0.0 +0,0.977100,0.0,0.0,0.0,0.0,0.0 +0,0.977200,0.0,0.0,0.0,0.0,0.0 +0,0.977300,0.0,0.0,0.0,0.0,0.0 +0,0.977400,0.0,0.0,0.0,0.0,0.0 +0,0.977500,0.0,0.0,0.0,0.0,0.0 +0,0.977600,0.0,0.0,0.0,0.0,0.0 +0,0.977700,0.0,0.0,0.0,0.0,0.0 +0,0.977800,0.0,0.0,0.0,0.0,0.0 +0,0.977900,0.0,0.0,0.0,0.0,0.0 +0,0.978000,0.0,0.0,0.0,0.0,0.0 +0,0.978100,0.0,0.0,0.0,0.0,0.0 +0,0.978200,0.0,0.0,0.0,0.0,0.0 +0,0.978300,0.0,0.0,0.0,0.0,0.0 +0,0.978400,0.0,0.0,0.0,0.0,0.0 +0,0.978500,0.0,0.0,0.0,0.0,0.0 +0,0.978600,0.0,0.0,0.0,0.0,0.0 +0,0.978700,0.0,0.0,0.0,0.0,0.0 +0,0.978800,0.0,0.0,0.0,0.0,0.0 +0,0.978900,0.0,0.0,0.0,0.0,0.0 +0,0.979000,0.0,0.0,0.0,0.0,0.0 +0,0.979100,0.0,0.0,0.0,0.0,0.0 +0,0.979200,0.0,0.0,0.0,0.0,0.0 +0,0.979300,0.0,0.0,0.0,0.0,0.0 +0,0.979400,0.0,0.0,0.0,0.0,0.0 +0,0.979500,0.0,0.0,0.0,0.0,0.0 +0,0.979600,0.0,0.0,0.0,0.0,0.0 +0,0.979700,0.0,0.0,0.0,0.0,0.0 +0,0.979800,0.0,0.0,0.0,0.0,0.0 +0,0.979900,0.0,0.0,0.0,0.0,0.0 +0,0.980000,0.0,0.0,0.0,0.0,0.0 +0,0.980100,0.0,0.0,0.0,0.0,0.0 +1,392.343435,0.0,0.0,0.0,0.0,0.0,1.0 +0,0.980200,0.0,0.0,0.0,0.0,0.0 +0,0.980300,0.0,0.0,0.0,0.0,0.0 +0,0.980400,0.0,0.0,0.0,0.0,0.0 +0,0.980500,0.0,0.0,0.0,0.0,0.0 +0,0.980600,0.0,0.0,0.0,0.0,0.0 +0,0.980700,0.0,0.0,0.0,0.0,0.0 +0,0.980800,0.0,0.0,0.0,0.0,0.0 +0,0.980900,0.0,0.0,0.0,0.0,0.0 +0,0.981000,0.0,0.0,0.0,0.0,0.0 +0,0.981100,0.0,0.0,0.0,0.0,0.0 +0,0.981200,0.0,0.0,0.0,0.0,0.0 +0,0.981300,0.0,0.0,0.0,0.0,0.0 +0,0.981400,0.0,0.0,0.0,0.0,0.0 +0,0.981500,0.0,0.0,0.0,0.0,0.0 +0,0.981600,0.0,0.0,0.0,0.0,0.0 +0,0.981700,0.0,0.0,0.0,0.0,0.0 +0,0.981800,0.0,0.0,0.0,0.0,0.0 +0,0.981900,0.0,0.0,0.0,0.0,0.0 +0,0.982000,0.0,0.0,0.0,0.0,0.0 +0,0.982100,0.0,0.0,0.0,0.0,0.0 +0,0.982200,0.0,0.0,0.0,0.0,0.0 +0,0.982300,0.0,0.0,0.0,0.0,0.0 +0,0.982400,0.0,0.0,0.0,0.0,0.0 +0,0.982500,0.0,0.0,0.0,0.0,0.0 +0,0.982600,0.0,0.0,0.0,0.0,0.0 +0,0.982700,0.0,0.0,0.0,0.0,0.0 +0,0.982800,0.0,0.0,0.0,0.0,0.0 +0,0.982900,0.0,0.0,0.0,0.0,0.0 +0,0.983000,0.0,0.0,0.0,0.0,0.0 +0,0.983100,0.0,0.0,0.0,0.0,0.0 +0,0.983200,0.0,0.0,0.0,0.0,0.0 +0,0.983300,0.0,0.0,0.0,0.0,0.0 +0,0.983400,0.0,0.0,0.0,0.0,0.0 +0,0.983500,0.0,0.0,0.0,0.0,0.0 +0,0.983600,0.0,0.0,0.0,0.0,0.0 +0,0.983700,0.0,0.0,0.0,0.0,0.0 +0,0.983800,0.0,0.0,0.0,0.0,0.0 +0,0.983900,0.0,0.0,0.0,0.0,0.0 +0,0.984000,0.0,0.0,0.0,0.0,0.0 +0,0.984100,0.0,0.0,0.0,0.0,0.0 +0,0.984200,0.0,0.0,0.0,0.0,0.0 +0,0.984300,0.0,0.0,0.0,0.0,0.0 +0,0.984400,0.0,0.0,0.0,0.0,0.0 +0,0.984500,0.0,0.0,0.0,0.0,0.0 +0,0.984600,0.0,0.0,0.0,0.0,0.0 +0,0.984700,0.0,0.0,0.0,0.0,0.0 +0,0.984800,0.0,0.0,0.0,0.0,0.0 +0,0.984900,0.0,0.0,0.0,0.0,0.0 +0,0.985000,0.0,0.0,0.0,0.0,0.0 +0,0.985100,0.0,0.0,0.0,0.0,0.0 +0,0.985200,0.0,0.0,0.0,0.0,0.0 +0,0.985300,0.0,0.0,0.0,0.0,0.0 +0,0.985400,0.0,0.0,0.0,0.0,0.0 +0,0.985500,0.0,0.0,0.0,0.0,0.0 +0,0.985600,0.0,0.0,0.0,0.0,0.0 +0,0.985700,0.0,0.0,0.0,0.0,0.0 +0,0.985800,0.0,0.0,0.0,0.0,0.0 +0,0.985900,0.0,0.0,0.0,0.0,0.0 +0,0.986000,0.0,0.0,0.0,0.0,0.0 +0,0.986100,0.0,0.0,0.0,0.0,0.0 +0,0.986200,0.0,0.0,0.0,0.0,0.0 +0,0.986300,0.0,0.0,0.0,0.0,0.0 +0,0.986400,0.0,0.0,0.0,0.0,0.0 +0,0.986500,0.0,0.0,0.0,0.0,0.0 +0,0.986600,0.0,0.0,0.0,0.0,0.0 +0,0.986700,0.0,0.0,0.0,0.0,0.0 +0,0.986800,0.0,0.0,0.0,0.0,0.0 +0,0.986900,0.0,0.0,0.0,0.0,0.0 +0,0.987000,0.0,0.0,0.0,0.0,0.0 +0,0.987100,0.0,0.0,0.0,0.0,0.0 +0,0.987200,0.0,0.0,0.0,0.0,0.0 +0,0.987300,0.0,0.0,0.0,0.0,0.0 +0,0.987400,0.0,0.0,0.0,0.0,0.0 +0,0.987500,0.0,0.0,0.0,0.0,0.0 +0,0.987600,0.0,0.0,0.0,0.0,0.0 +0,0.987700,0.0,0.0,0.0,0.0,0.0 +0,0.987800,0.0,0.0,0.0,0.0,0.0 +0,0.987900,0.0,0.0,0.0,0.0,0.0 +0,0.988000,0.0,0.0,0.0,0.0,0.0 +0,0.988100,0.0,0.0,0.0,0.0,0.0 +0,0.988200,0.0,0.0,0.0,0.0,0.0 +0,0.988300,0.0,0.0,0.0,0.0,0.0 +0,0.988400,0.0,0.0,0.0,0.0,0.0 +0,0.988500,0.0,0.0,0.0,0.0,0.0 +0,0.988600,0.0,0.0,0.0,0.0,0.0 +0,0.988700,0.0,0.0,0.0,0.0,0.0 +0,0.988800,0.0,0.0,0.0,0.0,0.0 +0,0.988900,0.0,0.0,0.0,0.0,0.0 +0,0.989000,0.0,0.0,0.0,0.0,0.0 +0,0.989100,0.0,0.0,0.0,0.0,0.0 +0,0.989200,0.0,0.0,0.0,0.0,0.0 +0,0.989300,0.0,0.0,0.0,0.0,0.0 +0,0.989400,0.0,0.0,0.0,0.0,0.0 +0,0.989500,0.0,0.0,0.0,0.0,0.0 +0,0.989600,0.0,0.0,0.0,0.0,0.0 +0,0.989700,0.0,0.0,0.0,0.0,0.0 +0,0.989800,0.0,0.0,0.0,0.0,0.0 +0,0.989900,0.0,0.0,0.0,0.0,0.0 +0,0.990000,0.0,0.0,0.0,0.0,0.0 +0,0.990100,0.0,0.0,0.0,0.0,0.0 +0,0.990200,0.0,0.0,0.0,0.0,0.0 +0,0.990300,0.0,0.0,0.0,0.0,0.0 +0,0.990400,0.0,0.0,0.0,0.0,0.0 +0,0.990500,0.0,0.0,0.0,0.0,0.0 +0,0.990600,0.0,0.0,0.0,0.0,0.0 +0,0.990700,0.0,0.0,0.0,0.0,0.0 +0,0.990800,0.0,0.0,0.0,0.0,0.0 +0,0.990900,0.0,0.0,0.0,0.0,0.0 +0,0.991000,0.0,0.0,0.0,0.0,0.0 +0,0.991100,0.0,0.0,0.0,0.0,0.0 +0,0.991200,0.0,0.0,0.0,0.0,0.0 +0,0.991300,0.0,0.0,0.0,0.0,0.0 +0,0.991400,0.0,0.0,0.0,0.0,0.0 +0,0.991500,0.0,0.0,0.0,0.0,0.0 +0,0.991600,0.0,0.0,0.0,0.0,0.0 +0,0.991700,0.0,0.0,0.0,0.0,0.0 +0,0.991800,0.0,0.0,0.0,0.0,0.0 +0,0.991900,0.0,0.0,0.0,0.0,0.0 +0,0.992000,0.0,0.0,0.0,0.0,0.0 +0,0.992100,0.0,0.0,0.0,0.0,0.0 +0,0.992200,0.0,0.0,0.0,0.0,0.0 +0,0.992300,0.0,0.0,0.0,0.0,0.0 +0,0.992400,0.0,0.0,0.0,0.0,0.0 +0,0.992500,0.0,0.0,0.0,0.0,0.0 +0,0.992600,0.0,0.0,0.0,0.0,0.0 +0,0.992700,0.0,0.0,0.0,0.0,0.0 +0,0.992800,0.0,0.0,0.0,0.0,0.0 +0,0.992900,0.0,0.0,0.0,0.0,0.0 +0,0.993000,0.0,0.0,0.0,0.0,0.0 +0,0.993100,0.0,0.0,0.0,0.0,0.0 +0,0.993200,0.0,0.0,0.0,0.0,0.0 +0,0.993300,0.0,0.0,0.0,0.0,0.0 +0,0.993400,0.0,0.0,0.0,0.0,0.0 +0,0.993500,0.0,0.0,0.0,0.0,0.0 +0,0.993600,0.0,0.0,0.0,0.0,0.0 +0,0.993700,0.0,0.0,0.0,0.0,0.0 +0,0.993800,0.0,0.0,0.0,0.0,0.0 +0,0.993900,0.0,0.0,0.0,0.0,0.0 +0,0.994000,0.0,0.0,0.0,0.0,0.0 +0,0.994100,0.0,0.0,0.0,0.0,0.0 +0,0.994200,0.0,0.0,0.0,0.0,0.0 +0,0.994300,0.0,0.0,0.0,0.0,0.0 +0,0.994400,0.0,0.0,0.0,0.0,0.0 +0,0.994500,0.0,0.0,0.0,0.0,0.0 +0,0.994600,0.0,0.0,0.0,0.0,0.0 +0,0.994700,0.0,0.0,0.0,0.0,0.0 +0,0.994800,0.0,0.0,0.0,0.0,0.0 +0,0.994900,0.0,0.0,0.0,0.0,0.0 +0,0.995000,0.0,0.0,0.0,0.0,0.0 +0,0.995100,0.0,0.0,0.0,0.0,0.0 +0,0.995200,0.0,0.0,0.0,0.0,0.0 +0,0.995300,0.0,0.0,0.0,0.0,0.0 +0,0.995400,0.0,0.0,0.0,0.0,0.0 +0,0.995500,0.0,0.0,0.0,0.0,0.0 +0,0.995600,0.0,0.0,0.0,0.0,0.0 +0,0.995700,0.0,0.0,0.0,0.0,0.0 +0,0.995800,0.0,0.0,0.0,0.0,0.0 +0,0.995900,0.0,0.0,0.0,0.0,0.0 +0,0.996000,0.0,0.0,0.0,0.0,0.0 +0,0.996100,0.0,0.0,0.0,0.0,0.0 +0,0.996200,0.0,0.0,0.0,0.0,0.0 +0,0.996300,0.0,0.0,0.0,0.0,0.0 +0,0.996400,0.0,0.0,0.0,0.0,0.0 +0,0.996500,0.0,0.0,0.0,0.0,0.0 +0,0.996600,0.0,0.0,0.0,0.0,0.0 +0,0.996700,0.0,0.0,0.0,0.0,0.0 +0,0.996800,0.0,0.0,0.0,0.0,0.0 +0,0.996900,0.0,0.0,0.0,0.0,0.0 +0,0.997000,0.0,0.0,0.0,0.0,0.0 +0,0.997100,0.0,0.0,0.0,0.0,0.0 +0,0.997200,0.0,0.0,0.0,0.0,0.0 +0,0.997300,0.0,0.0,0.0,0.0,0.0 +0,0.997400,0.0,0.0,0.0,0.0,0.0 +0,0.997500,0.0,0.0,0.0,0.0,0.0 +0,0.997600,0.0,0.0,0.0,0.0,0.0 +0,0.997700,0.0,0.0,0.0,0.0,0.0 +0,0.997800,0.0,0.0,0.0,0.0,0.0 +0,0.997900,0.0,0.0,0.0,0.0,0.0 +0,0.998000,0.0,0.0,0.0,0.0,0.0 +0,0.998100,0.0,0.0,0.0,0.0,0.0 +0,0.998200,0.0,0.0,0.0,0.0,0.0 +0,0.998300,0.0,0.0,0.0,0.0,0.0 +0,0.998400,0.0,0.0,0.0,0.0,0.0 +0,0.998500,0.0,0.0,0.0,0.0,0.0 +0,0.998600,0.0,0.0,0.0,0.0,0.0 +0,0.998700,0.0,0.0,0.0,0.0,0.0 +0,0.998800,0.0,0.0,0.0,0.0,0.0 +0,0.998900,0.0,0.0,0.0,0.0,0.0 +0,0.999000,0.0,0.0,0.0,0.0,0.0 +0,0.999100,0.0,0.0,0.0,0.0,0.0 +0,0.999200,0.0,0.0,0.0,0.0,0.0 +0,0.999300,0.0,0.0,0.0,0.0,0.0 +0,0.999400,0.0,0.0,0.0,0.0,0.0 +0,0.999500,0.0,0.0,0.0,0.0,0.0 +0,0.999600,0.0,0.0,0.0,0.0,0.0 +0,0.999700,0.0,0.0,0.0,0.0,0.0 +0,0.999800,0.0,0.0,0.0,0.0,0.0 +0,0.999900,0.0,0.0,0.0,0.0,0.0 +0,1.000000,0.0,0.0,0.0,0.0,0.0 +0,1.000100,0.0,0.0,0.0,0.0,0.0 +1,416.854194,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.000200,0.0,0.0,0.0,0.0,0.0 +0,1.000300,0.0,0.0,0.0,0.0,0.0 +0,1.000400,0.0,0.0,0.0,0.0,0.0 +0,1.000500,0.0,0.0,0.0,0.0,0.0 +0,1.000600,0.0,0.0,0.0,0.0,0.0 +0,1.000700,0.0,0.0,0.0,0.0,0.0 +0,1.000800,0.0,0.0,0.0,0.0,0.0 +0,1.000900,0.0,0.0,0.0,0.0,0.0 +0,1.001000,0.0,0.0,0.0,0.0,0.0 +0,1.001100,0.0,0.0,0.0,0.0,0.0 +0,1.001200,0.0,0.0,0.0,0.0,0.0 +0,1.001300,0.0,0.0,0.0,0.0,0.0 +0,1.001400,0.0,0.0,0.0,0.0,0.0 +0,1.001500,0.0,0.0,0.0,0.0,0.0 +0,1.001600,0.0,0.0,0.0,0.0,0.0 +0,1.001700,0.0,0.0,0.0,0.0,0.0 +0,1.001800,0.0,0.0,0.0,0.0,0.0 +0,1.001900,0.0,0.0,0.0,0.0,0.0 +0,1.002000,0.0,0.0,0.0,0.0,0.0 +0,1.002100,0.0,0.0,0.0,0.0,0.0 +0,1.002200,0.0,0.0,0.0,0.0,0.0 +0,1.002300,0.0,0.0,0.0,0.0,0.0 +0,1.002400,0.0,0.0,0.0,0.0,0.0 +0,1.002500,0.0,0.0,0.0,0.0,0.0 +0,1.002600,0.0,0.0,0.0,0.0,0.0 +0,1.002700,0.0,0.0,0.0,0.0,0.0 +0,1.002800,0.0,0.0,0.0,0.0,0.0 +0,1.002900,0.0,0.0,0.0,0.0,0.0 +0,1.003000,0.0,0.0,0.0,0.0,0.0 +0,1.003100,0.0,0.0,0.0,0.0,0.0 +0,1.003200,0.0,0.0,0.0,0.0,0.0 +0,1.003300,0.0,0.0,0.0,0.0,0.0 +0,1.003400,0.0,0.0,0.0,0.0,0.0 +0,1.003500,0.0,0.0,0.0,0.0,0.0 +0,1.003600,0.0,0.0,0.0,0.0,0.0 +0,1.003700,0.0,0.0,0.0,0.0,0.0 +0,1.003800,0.0,0.0,0.0,0.0,0.0 +0,1.003900,0.0,0.0,0.0,0.0,0.0 +0,1.004000,0.0,0.0,0.0,0.0,0.0 +0,1.004100,0.0,0.0,0.0,0.0,0.0 +0,1.004200,0.0,0.0,0.0,0.0,0.0 +0,1.004300,0.0,0.0,0.0,0.0,0.0 +0,1.004400,0.0,0.0,0.0,0.0,0.0 +0,1.004500,0.0,0.0,0.0,0.0,0.0 +0,1.004600,0.0,0.0,0.0,0.0,0.0 +0,1.004700,0.0,0.0,0.0,0.0,0.0 +0,1.004800,0.0,0.0,0.0,0.0,0.0 +0,1.004900,0.0,0.0,0.0,0.0,0.0 +0,1.005000,0.0,0.0,0.0,0.0,0.0 +0,1.005100,0.0,0.0,0.0,0.0,0.0 +0,1.005200,0.0,0.0,0.0,0.0,0.0 +0,1.005300,0.0,0.0,0.0,0.0,0.0 +0,1.005400,0.0,0.0,0.0,0.0,0.0 +0,1.005500,0.0,0.0,0.0,0.0,0.0 +0,1.005600,0.0,0.0,0.0,0.0,0.0 +0,1.005700,0.0,0.0,0.0,0.0,0.0 +0,1.005800,0.0,0.0,0.0,0.0,0.0 +0,1.005900,0.0,0.0,0.0,0.0,0.0 +0,1.006000,0.0,0.0,0.0,0.0,0.0 +0,1.006100,0.0,0.0,0.0,0.0,0.0 +0,1.006200,0.0,0.0,0.0,0.0,0.0 +0,1.006300,0.0,0.0,0.0,0.0,0.0 +0,1.006400,0.0,0.0,0.0,0.0,0.0 +0,1.006500,0.0,0.0,0.0,0.0,0.0 +0,1.006600,0.0,0.0,0.0,0.0,0.0 +0,1.006700,0.0,0.0,0.0,0.0,0.0 +0,1.006800,0.0,0.0,0.0,0.0,0.0 +0,1.006900,0.0,0.0,0.0,0.0,0.0 +0,1.007000,0.0,0.0,0.0,0.0,0.0 +0,1.007100,0.0,0.0,0.0,0.0,0.0 +0,1.007200,0.0,0.0,0.0,0.0,0.0 +0,1.007300,0.0,0.0,0.0,0.0,0.0 +0,1.007400,0.0,0.0,0.0,0.0,0.0 +0,1.007500,0.0,0.0,0.0,0.0,0.0 +0,1.007600,0.0,0.0,0.0,0.0,0.0 +0,1.007700,0.0,0.0,0.0,0.0,0.0 +0,1.007800,0.0,0.0,0.0,0.0,0.0 +0,1.007900,0.0,0.0,0.0,0.0,0.0 +0,1.008000,0.0,0.0,0.0,0.0,0.0 +0,1.008100,0.0,0.0,0.0,0.0,0.0 +0,1.008200,0.0,0.0,0.0,0.0,0.0 +0,1.008300,0.0,0.0,0.0,0.0,0.0 +0,1.008400,0.0,0.0,0.0,0.0,0.0 +0,1.008500,0.0,0.0,0.0,0.0,0.0 +0,1.008600,0.0,0.0,0.0,0.0,0.0 +0,1.008700,0.0,0.0,0.0,0.0,0.0 +0,1.008800,0.0,0.0,0.0,0.0,0.0 +0,1.008900,0.0,0.0,0.0,0.0,0.0 +0,1.009000,0.0,0.0,0.0,0.0,0.0 +0,1.009100,0.0,0.0,0.0,0.0,0.0 +0,1.009200,0.0,0.0,0.0,0.0,0.0 +0,1.009300,0.0,0.0,0.0,0.0,0.0 +0,1.009400,0.0,0.0,0.0,0.0,0.0 +0,1.009500,0.0,0.0,0.0,0.0,0.0 +0,1.009600,0.0,0.0,0.0,0.0,0.0 +0,1.009700,0.0,0.0,0.0,0.0,0.0 +0,1.009800,0.0,0.0,0.0,0.0,0.0 +0,1.009900,0.0,0.0,0.0,0.0,0.0 +0,1.010000,0.0,0.0,0.0,0.0,0.0 +0,1.010100,0.0,0.0,0.0,0.0,0.0 +0,1.010200,0.0,0.0,0.0,0.0,0.0 +0,1.010300,0.0,0.0,0.0,0.0,0.0 +0,1.010400,0.0,0.0,0.0,0.0,0.0 +0,1.010500,0.0,0.0,0.0,0.0,0.0 +0,1.010600,0.0,0.0,0.0,0.0,0.0 +0,1.010700,0.0,0.0,0.0,0.0,0.0 +0,1.010800,0.0,0.0,0.0,0.0,0.0 +0,1.010900,0.0,0.0,0.0,0.0,0.0 +0,1.011000,0.0,0.0,0.0,0.0,0.0 +0,1.011100,0.0,0.0,0.0,0.0,0.0 +0,1.011200,0.0,0.0,0.0,0.0,0.0 +0,1.011300,0.0,0.0,0.0,0.0,0.0 +0,1.011400,0.0,0.0,0.0,0.0,0.0 +0,1.011500,0.0,0.0,0.0,0.0,0.0 +0,1.011600,0.0,0.0,0.0,0.0,0.0 +0,1.011700,0.0,0.0,0.0,0.0,0.0 +0,1.011800,0.0,0.0,0.0,0.0,0.0 +0,1.011900,0.0,0.0,0.0,0.0,0.0 +0,1.012000,0.0,0.0,0.0,0.0,0.0 +0,1.012100,0.0,0.0,0.0,0.0,0.0 +0,1.012200,0.0,0.0,0.0,0.0,0.0 +0,1.012300,0.0,0.0,0.0,0.0,0.0 +0,1.012400,0.0,0.0,0.0,0.0,0.0 +0,1.012500,0.0,0.0,0.0,0.0,0.0 +0,1.012600,0.0,0.0,0.0,0.0,0.0 +0,1.012700,0.0,0.0,0.0,0.0,0.0 +0,1.012800,0.0,0.0,0.0,0.0,0.0 +0,1.012900,0.0,0.0,0.0,0.0,0.0 +0,1.013000,0.0,0.0,0.0,0.0,0.0 +0,1.013100,0.0,0.0,0.0,0.0,0.0 +0,1.013200,0.0,0.0,0.0,0.0,0.0 +0,1.013300,0.0,0.0,0.0,0.0,0.0 +0,1.013400,0.0,0.0,0.0,0.0,0.0 +0,1.013500,0.0,0.0,0.0,0.0,0.0 +0,1.013600,0.0,0.0,0.0,0.0,0.0 +0,1.013700,0.0,0.0,0.0,0.0,0.0 +0,1.013800,0.0,0.0,0.0,0.0,0.0 +0,1.013900,0.0,0.0,0.0,0.0,0.0 +0,1.014000,0.0,0.0,0.0,0.0,0.0 +0,1.014100,0.0,0.0,0.0,0.0,0.0 +0,1.014200,0.0,0.0,0.0,0.0,0.0 +0,1.014300,0.0,0.0,0.0,0.0,0.0 +0,1.014400,0.0,0.0,0.0,0.0,0.0 +0,1.014500,0.0,0.0,0.0,0.0,0.0 +0,1.014600,0.0,0.0,0.0,0.0,0.0 +0,1.014700,0.0,0.0,0.0,0.0,0.0 +0,1.014800,0.0,0.0,0.0,0.0,0.0 +0,1.014900,0.0,0.0,0.0,0.0,0.0 +0,1.015000,0.0,0.0,0.0,0.0,0.0 +0,1.015100,0.0,0.0,0.0,0.0,0.0 +0,1.015200,0.0,0.0,0.0,0.0,0.0 +0,1.015300,0.0,0.0,0.0,0.0,0.0 +0,1.015400,0.0,0.0,0.0,0.0,0.0 +0,1.015500,0.0,0.0,0.0,0.0,0.0 +0,1.015600,0.0,0.0,0.0,0.0,0.0 +0,1.015700,0.0,0.0,0.0,0.0,0.0 +0,1.015800,0.0,0.0,0.0,0.0,0.0 +0,1.015900,0.0,0.0,0.0,0.0,0.0 +0,1.016000,0.0,0.0,0.0,0.0,0.0 +0,1.016100,0.0,0.0,0.0,0.0,0.0 +0,1.016200,0.0,0.0,0.0,0.0,0.0 +0,1.016300,0.0,0.0,0.0,0.0,0.0 +0,1.016400,0.0,0.0,0.0,0.0,0.0 +0,1.016500,0.0,0.0,0.0,0.0,0.0 +0,1.016600,0.0,0.0,0.0,0.0,0.0 +0,1.016700,0.0,0.0,0.0,0.0,0.0 +0,1.016800,0.0,0.0,0.0,0.0,0.0 +0,1.016900,0.0,0.0,0.0,0.0,0.0 +0,1.017000,0.0,0.0,0.0,0.0,0.0 +0,1.017100,0.0,0.0,0.0,0.0,0.0 +0,1.017200,0.0,0.0,0.0,0.0,0.0 +0,1.017300,0.0,0.0,0.0,0.0,0.0 +0,1.017400,0.0,0.0,0.0,0.0,0.0 +0,1.017500,0.0,0.0,0.0,0.0,0.0 +0,1.017600,0.0,0.0,0.0,0.0,0.0 +0,1.017700,0.0,0.0,0.0,0.0,0.0 +0,1.017800,0.0,0.0,0.0,0.0,0.0 +0,1.017900,0.0,0.0,0.0,0.0,0.0 +0,1.018000,0.0,0.0,0.0,0.0,0.0 +0,1.018100,0.0,0.0,0.0,0.0,0.0 +0,1.018200,0.0,0.0,0.0,0.0,0.0 +0,1.018300,0.0,0.0,0.0,0.0,0.0 +0,1.018400,0.0,0.0,0.0,0.0,0.0 +0,1.018500,0.0,0.0,0.0,0.0,0.0 +0,1.018600,0.0,0.0,0.0,0.0,0.0 +0,1.018700,0.0,0.0,0.0,0.0,0.0 +0,1.018800,0.0,0.0,0.0,0.0,0.0 +0,1.018900,0.0,0.0,0.0,0.0,0.0 +0,1.019000,0.0,0.0,0.0,0.0,0.0 +0,1.019100,0.0,0.0,0.0,0.0,0.0 +0,1.019200,0.0,0.0,0.0,0.0,0.0 +0,1.019300,0.0,0.0,0.0,0.0,0.0 +0,1.019400,0.0,0.0,0.0,0.0,0.0 +0,1.019500,0.0,0.0,0.0,0.0,0.0 +0,1.019600,0.0,0.0,0.0,0.0,0.0 +0,1.019700,0.0,0.0,0.0,0.0,0.0 +0,1.019800,0.0,0.0,0.0,0.0,0.0 +0,1.019900,0.0,0.0,0.0,0.0,0.0 +0,1.020000,0.0,0.0,0.0,0.0,0.0 +0,1.020100,0.0,0.0,0.0,0.0,0.0 +1,442.365103,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.020200,0.0,0.0,0.0,0.0,0.0 +0,1.020300,0.0,0.0,0.0,0.0,0.0 +0,1.020400,0.0,0.0,0.0,0.0,0.0 +0,1.020500,0.0,0.0,0.0,0.0,0.0 +0,1.020600,0.0,0.0,0.0,0.0,0.0 +0,1.020700,0.0,0.0,0.0,0.0,0.0 +0,1.020800,0.0,0.0,0.0,0.0,0.0 +0,1.020900,0.0,0.0,0.0,0.0,0.0 +0,1.021000,0.0,0.0,0.0,0.0,0.0 +0,1.021100,0.0,0.0,0.0,0.0,0.0 +0,1.021200,0.0,0.0,0.0,0.0,0.0 +0,1.021300,0.0,0.0,0.0,0.0,0.0 +0,1.021400,0.0,0.0,0.0,0.0,0.0 +0,1.021500,0.0,0.0,0.0,0.0,0.0 +0,1.021600,0.0,0.0,0.0,0.0,0.0 +0,1.021700,0.0,0.0,0.0,0.0,0.0 +0,1.021800,0.0,0.0,0.0,0.0,0.0 +0,1.021900,0.0,0.0,0.0,0.0,0.0 +0,1.022000,0.0,0.0,0.0,0.0,0.0 +0,1.022100,0.0,0.0,0.0,0.0,0.0 +0,1.022200,0.0,0.0,0.0,0.0,0.0 +0,1.022300,0.0,0.0,0.0,0.0,0.0 +0,1.022400,0.0,0.0,0.0,0.0,0.0 +0,1.022500,0.0,0.0,0.0,0.0,0.0 +0,1.022600,0.0,0.0,0.0,0.0,0.0 +0,1.022700,0.0,0.0,0.0,0.0,0.0 +0,1.022800,0.0,0.0,0.0,0.0,0.0 +0,1.022900,0.0,0.0,0.0,0.0,0.0 +0,1.023000,0.0,0.0,0.0,0.0,0.0 +0,1.023100,0.0,0.0,0.0,0.0,0.0 +0,1.023200,0.0,0.0,0.0,0.0,0.0 +0,1.023300,0.0,0.0,0.0,0.0,0.0 +0,1.023400,0.0,0.0,0.0,0.0,0.0 +0,1.023500,0.0,0.0,0.0,0.0,0.0 +0,1.023600,0.0,0.0,0.0,0.0,0.0 +0,1.023700,0.0,0.0,0.0,0.0,0.0 +0,1.023800,0.0,0.0,0.0,0.0,0.0 +0,1.023900,0.0,0.0,0.0,0.0,0.0 +0,1.024000,0.0,0.0,0.0,0.0,0.0 +0,1.024100,0.0,0.0,0.0,0.0,0.0 +0,1.024200,0.0,0.0,0.0,0.0,0.0 +0,1.024300,0.0,0.0,0.0,0.0,0.0 +0,1.024400,0.0,0.0,0.0,0.0,0.0 +0,1.024500,0.0,0.0,0.0,0.0,0.0 +0,1.024600,0.0,0.0,0.0,0.0,0.0 +0,1.024700,0.0,0.0,0.0,0.0,0.0 +0,1.024800,0.0,0.0,0.0,0.0,0.0 +0,1.024900,0.0,0.0,0.0,0.0,0.0 +0,1.025000,0.0,0.0,0.0,0.0,0.0 +0,1.025100,0.0,0.0,0.0,0.0,0.0 +0,1.025200,0.0,0.0,0.0,0.0,0.0 +0,1.025300,0.0,0.0,0.0,0.0,0.0 +0,1.025400,0.0,0.0,0.0,0.0,0.0 +0,1.025500,0.0,0.0,0.0,0.0,0.0 +0,1.025600,0.0,0.0,0.0,0.0,0.0 +0,1.025700,0.0,0.0,0.0,0.0,0.0 +0,1.025800,0.0,0.0,0.0,0.0,0.0 +0,1.025900,0.0,0.0,0.0,0.0,0.0 +0,1.026000,0.0,0.0,0.0,0.0,0.0 +0,1.026100,0.0,0.0,0.0,0.0,0.0 +0,1.026200,0.0,0.0,0.0,0.0,0.0 +0,1.026300,0.0,0.0,0.0,0.0,0.0 +0,1.026400,0.0,0.0,0.0,0.0,0.0 +0,1.026500,0.0,0.0,0.0,0.0,0.0 +0,1.026600,0.0,0.0,0.0,0.0,0.0 +0,1.026700,0.0,0.0,0.0,0.0,0.0 +0,1.026800,0.0,0.0,0.0,0.0,0.0 +0,1.026900,0.0,0.0,0.0,0.0,0.0 +0,1.027000,0.0,0.0,0.0,0.0,0.0 +0,1.027100,0.0,0.0,0.0,0.0,0.0 +0,1.027200,0.0,0.0,0.0,0.0,0.0 +0,1.027300,0.0,0.0,0.0,0.0,0.0 +0,1.027400,0.0,0.0,0.0,0.0,0.0 +0,1.027500,0.0,0.0,0.0,0.0,0.0 +0,1.027600,0.0,0.0,0.0,0.0,0.0 +0,1.027700,0.0,0.0,0.0,0.0,0.0 +0,1.027800,0.0,0.0,0.0,0.0,0.0 +0,1.027900,0.0,0.0,0.0,0.0,0.0 +0,1.028000,0.0,0.0,0.0,0.0,0.0 +0,1.028100,0.0,0.0,0.0,0.0,0.0 +0,1.028200,0.0,0.0,0.0,0.0,0.0 +0,1.028300,0.0,0.0,0.0,0.0,0.0 +0,1.028400,0.0,0.0,0.0,0.0,0.0 +0,1.028500,0.0,0.0,0.0,0.0,0.0 +0,1.028600,0.0,0.0,0.0,0.0,0.0 +0,1.028700,0.0,0.0,0.0,0.0,0.0 +0,1.028800,0.0,0.0,0.0,0.0,0.0 +0,1.028900,0.0,0.0,0.0,0.0,0.0 +0,1.029000,0.0,0.0,0.0,0.0,0.0 +0,1.029100,0.0,0.0,0.0,0.0,0.0 +0,1.029200,0.0,0.0,0.0,0.0,0.0 +0,1.029300,0.0,0.0,0.0,0.0,0.0 +0,1.029400,0.0,0.0,0.0,0.0,0.0 +0,1.029500,0.0,0.0,0.0,0.0,0.0 +0,1.029600,0.0,0.0,0.0,0.0,0.0 +0,1.029700,0.0,0.0,0.0,0.0,0.0 +0,1.029800,0.0,0.0,0.0,0.0,0.0 +0,1.029900,0.0,0.0,0.0,0.0,0.0 +0,1.030000,0.0,0.0,0.0,0.0,0.0 +0,1.030100,0.0,0.0,0.0,0.0,0.0 +0,1.030200,0.0,0.0,0.0,0.0,0.0 +0,1.030300,0.0,0.0,0.0,0.0,0.0 +0,1.030400,0.0,0.0,0.0,0.0,0.0 +0,1.030500,0.0,0.0,0.0,0.0,0.0 +0,1.030600,0.0,0.0,0.0,0.0,0.0 +0,1.030700,0.0,0.0,0.0,0.0,0.0 +0,1.030800,0.0,0.0,0.0,0.0,0.0 +0,1.030900,0.0,0.0,0.0,0.0,0.0 +0,1.031000,0.0,0.0,0.0,0.0,0.0 +0,1.031100,0.0,0.0,0.0,0.0,0.0 +0,1.031200,0.0,0.0,0.0,0.0,0.0 +0,1.031300,0.0,0.0,0.0,0.0,0.0 +0,1.031400,0.0,0.0,0.0,0.0,0.0 +0,1.031500,0.0,0.0,0.0,0.0,0.0 +0,1.031600,0.0,0.0,0.0,0.0,0.0 +0,1.031700,0.0,0.0,0.0,0.0,0.0 +0,1.031800,0.0,0.0,0.0,0.0,0.0 +0,1.031900,0.0,0.0,0.0,0.0,0.0 +0,1.032000,0.0,0.0,0.0,0.0,0.0 +0,1.032100,0.0,0.0,0.0,0.0,0.0 +0,1.032200,0.0,0.0,0.0,0.0,0.0 +0,1.032300,0.0,0.0,0.0,0.0,0.0 +0,1.032400,0.0,0.0,0.0,0.0,0.0 +0,1.032500,0.0,0.0,0.0,0.0,0.0 +0,1.032600,0.0,0.0,0.0,0.0,0.0 +0,1.032700,0.0,0.0,0.0,0.0,0.0 +0,1.032800,0.0,0.0,0.0,0.0,0.0 +0,1.032900,0.0,0.0,0.0,0.0,0.0 +0,1.033000,0.0,0.0,0.0,0.0,0.0 +0,1.033100,0.0,0.0,0.0,0.0,0.0 +0,1.033200,0.0,0.0,0.0,0.0,0.0 +0,1.033300,0.0,0.0,0.0,0.0,0.0 +0,1.033400,0.0,0.0,0.0,0.0,0.0 +0,1.033500,0.0,0.0,0.0,0.0,0.0 +0,1.033600,0.0,0.0,0.0,0.0,0.0 +0,1.033700,0.0,0.0,0.0,0.0,0.0 +0,1.033800,0.0,0.0,0.0,0.0,0.0 +0,1.033900,0.0,0.0,0.0,0.0,0.0 +0,1.034000,0.0,0.0,0.0,0.0,0.0 +0,1.034100,0.0,0.0,0.0,0.0,0.0 +0,1.034200,0.0,0.0,0.0,0.0,0.0 +0,1.034300,0.0,0.0,0.0,0.0,0.0 +0,1.034400,0.0,0.0,0.0,0.0,0.0 +0,1.034500,0.0,0.0,0.0,0.0,0.0 +0,1.034600,0.0,0.0,0.0,0.0,0.0 +0,1.034700,0.0,0.0,0.0,0.0,0.0 +0,1.034800,0.0,0.0,0.0,0.0,0.0 +0,1.034900,0.0,0.0,0.0,0.0,0.0 +0,1.035000,0.0,0.0,0.0,0.0,0.0 +0,1.035100,0.0,0.0,0.0,0.0,0.0 +0,1.035200,0.0,0.0,0.0,0.0,0.0 +0,1.035300,0.0,0.0,0.0,0.0,0.0 +0,1.035400,0.0,0.0,0.0,0.0,0.0 +0,1.035500,0.0,0.0,0.0,0.0,0.0 +0,1.035600,0.0,0.0,0.0,0.0,0.0 +0,1.035700,0.0,0.0,0.0,0.0,0.0 +0,1.035800,0.0,0.0,0.0,0.0,0.0 +0,1.035900,0.0,0.0,0.0,0.0,0.0 +0,1.036000,0.0,0.0,0.0,0.0,0.0 +0,1.036100,0.0,0.0,0.0,0.0,0.0 +0,1.036200,0.0,0.0,0.0,0.0,0.0 +0,1.036300,0.0,0.0,0.0,0.0,0.0 +0,1.036400,0.0,0.0,0.0,0.0,0.0 +0,1.036500,0.0,0.0,0.0,0.0,0.0 +0,1.036600,0.0,0.0,0.0,0.0,0.0 +0,1.036700,0.0,0.0,0.0,0.0,0.0 +0,1.036800,0.0,0.0,0.0,0.0,0.0 +0,1.036900,0.0,0.0,0.0,0.0,0.0 +0,1.037000,0.0,0.0,0.0,0.0,0.0 +0,1.037100,0.0,0.0,0.0,0.0,0.0 +0,1.037200,0.0,0.0,0.0,0.0,0.0 +0,1.037300,0.0,0.0,0.0,0.0,0.0 +0,1.037400,0.0,0.0,0.0,0.0,0.0 +0,1.037500,0.0,0.0,0.0,0.0,0.0 +0,1.037600,0.0,0.0,0.0,0.0,0.0 +0,1.037700,0.0,0.0,0.0,0.0,0.0 +0,1.037800,0.0,0.0,0.0,0.0,0.0 +0,1.037900,0.0,0.0,0.0,0.0,0.0 +0,1.038000,0.0,0.0,0.0,0.0,0.0 +0,1.038100,0.0,0.0,0.0,0.0,0.0 +0,1.038200,0.0,0.0,0.0,0.0,0.0 +0,1.038300,0.0,0.0,0.0,0.0,0.0 +0,1.038400,0.0,0.0,0.0,0.0,0.0 +0,1.038500,0.0,0.0,0.0,0.0,0.0 +0,1.038600,0.0,0.0,0.0,0.0,0.0 +0,1.038700,0.0,0.0,0.0,0.0,0.0 +0,1.038800,0.0,0.0,0.0,0.0,0.0 +0,1.038900,0.0,0.0,0.0,0.0,0.0 +0,1.039000,0.0,0.0,0.0,0.0,0.0 +0,1.039100,0.0,0.0,0.0,0.0,0.0 +0,1.039200,0.0,0.0,0.0,0.0,0.0 +0,1.039300,0.0,0.0,0.0,0.0,0.0 +0,1.039400,0.0,0.0,0.0,0.0,0.0 +0,1.039500,0.0,0.0,0.0,0.0,0.0 +0,1.039600,0.0,0.0,0.0,0.0,0.0 +0,1.039700,0.0,0.0,0.0,0.0,0.0 +0,1.039800,0.0,0.0,0.0,0.0,0.0 +0,1.039900,0.0,0.0,0.0,0.0,0.0 +0,1.040000,0.0,0.0,0.0,0.0,0.0 +0,1.040100,0.0,0.0,0.0,0.0,0.0 +1,468.896162,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.040200,0.0,0.0,0.0,0.0,0.0 +0,1.040300,0.0,0.0,0.0,0.0,0.0 +0,1.040400,0.0,0.0,0.0,0.0,0.0 +0,1.040500,0.0,0.0,0.0,0.0,0.0 +0,1.040600,0.0,0.0,0.0,0.0,0.0 +0,1.040700,0.0,0.0,0.0,0.0,0.0 +0,1.040800,0.0,0.0,0.0,0.0,0.0 +0,1.040900,0.0,0.0,0.0,0.0,0.0 +0,1.041000,0.0,0.0,0.0,0.0,0.0 +0,1.041100,0.0,0.0,0.0,0.0,0.0 +0,1.041200,0.0,0.0,0.0,0.0,0.0 +0,1.041300,0.0,0.0,0.0,0.0,0.0 +0,1.041400,0.0,0.0,0.0,0.0,0.0 +0,1.041500,0.0,0.0,0.0,0.0,0.0 +0,1.041600,0.0,0.0,0.0,0.0,0.0 +0,1.041700,0.0,0.0,0.0,0.0,0.0 +0,1.041800,0.0,0.0,0.0,0.0,0.0 +0,1.041900,0.0,0.0,0.0,0.0,0.0 +0,1.042000,0.0,0.0,0.0,0.0,0.0 +0,1.042100,0.0,0.0,0.0,0.0,0.0 +0,1.042200,0.0,0.0,0.0,0.0,0.0 +0,1.042300,0.0,0.0,0.0,0.0,0.0 +0,1.042400,0.0,0.0,0.0,0.0,0.0 +0,1.042500,0.0,0.0,0.0,0.0,0.0 +0,1.042600,0.0,0.0,0.0,0.0,0.0 +0,1.042700,0.0,0.0,0.0,0.0,0.0 +0,1.042800,0.0,0.0,0.0,0.0,0.0 +0,1.042900,0.0,0.0,0.0,0.0,0.0 +0,1.043000,0.0,0.0,0.0,0.0,0.0 +0,1.043100,0.0,0.0,0.0,0.0,0.0 +0,1.043200,0.0,0.0,0.0,0.0,0.0 +0,1.043300,0.0,0.0,0.0,0.0,0.0 +0,1.043400,0.0,0.0,0.0,0.0,0.0 +0,1.043500,0.0,0.0,0.0,0.0,0.0 +0,1.043600,0.0,0.0,0.0,0.0,0.0 +0,1.043700,0.0,0.0,0.0,0.0,0.0 +0,1.043800,0.0,0.0,0.0,0.0,0.0 +0,1.043900,0.0,0.0,0.0,0.0,0.0 +0,1.044000,0.0,0.0,0.0,0.0,0.0 +0,1.044100,0.0,0.0,0.0,0.0,0.0 +0,1.044200,0.0,0.0,0.0,0.0,0.0 +0,1.044300,0.0,0.0,0.0,0.0,0.0 +0,1.044400,0.0,0.0,0.0,0.0,0.0 +0,1.044500,0.0,0.0,0.0,0.0,0.0 +0,1.044600,0.0,0.0,0.0,0.0,0.0 +0,1.044700,0.0,0.0,0.0,0.0,0.0 +0,1.044800,0.0,0.0,0.0,0.0,0.0 +0,1.044900,0.0,0.0,0.0,0.0,0.0 +0,1.045000,0.0,0.0,0.0,0.0,0.0 +0,1.045100,0.0,0.0,0.0,0.0,0.0 +0,1.045200,0.0,0.0,0.0,0.0,0.0 +0,1.045300,0.0,0.0,0.0,0.0,0.0 +0,1.045400,0.0,0.0,0.0,0.0,0.0 +0,1.045500,0.0,0.0,0.0,0.0,0.0 +0,1.045600,0.0,0.0,0.0,0.0,0.0 +0,1.045700,0.0,0.0,0.0,0.0,0.0 +0,1.045800,0.0,0.0,0.0,0.0,0.0 +0,1.045900,0.0,0.0,0.0,0.0,0.0 +0,1.046000,0.0,0.0,0.0,0.0,0.0 +0,1.046100,0.0,0.0,0.0,0.0,0.0 +0,1.046200,0.0,0.0,0.0,0.0,0.0 +0,1.046300,0.0,0.0,0.0,0.0,0.0 +0,1.046400,0.0,0.0,0.0,0.0,0.0 +0,1.046500,0.0,0.0,0.0,0.0,0.0 +0,1.046600,0.0,0.0,0.0,0.0,0.0 +0,1.046700,0.0,0.0,0.0,0.0,0.0 +0,1.046800,0.0,0.0,0.0,0.0,0.0 +0,1.046900,0.0,0.0,0.0,0.0,0.0 +0,1.047000,0.0,0.0,0.0,0.0,0.0 +0,1.047100,0.0,0.0,0.0,0.0,0.0 +0,1.047200,0.0,0.0,0.0,0.0,0.0 +0,1.047300,0.0,0.0,0.0,0.0,0.0 +0,1.047400,0.0,0.0,0.0,0.0,0.0 +0,1.047500,0.0,0.0,0.0,0.0,0.0 +0,1.047600,0.0,0.0,0.0,0.0,0.0 +0,1.047700,0.0,0.0,0.0,0.0,0.0 +0,1.047800,0.0,0.0,0.0,0.0,0.0 +0,1.047900,0.0,0.0,0.0,0.0,0.0 +0,1.048000,0.0,0.0,0.0,0.0,0.0 +0,1.048100,0.0,0.0,0.0,0.0,0.0 +0,1.048200,0.0,0.0,0.0,0.0,0.0 +0,1.048300,0.0,0.0,0.0,0.0,0.0 +0,1.048400,0.0,0.0,0.0,0.0,0.0 +0,1.048500,0.0,0.0,0.0,0.0,0.0 +0,1.048600,0.0,0.0,0.0,0.0,0.0 +0,1.048700,0.0,0.0,0.0,0.0,0.0 +0,1.048800,0.0,0.0,0.0,0.0,0.0 +0,1.048900,0.0,0.0,0.0,0.0,0.0 +0,1.049000,0.0,0.0,0.0,0.0,0.0 +0,1.049100,0.0,0.0,0.0,0.0,0.0 +0,1.049200,0.0,0.0,0.0,0.0,0.0 +0,1.049300,0.0,0.0,0.0,0.0,0.0 +0,1.049400,0.0,0.0,0.0,0.0,0.0 +0,1.049500,0.0,0.0,0.0,0.0,0.0 +0,1.049600,0.0,0.0,0.0,0.0,0.0 +0,1.049700,0.0,0.0,0.0,0.0,0.0 +0,1.049800,0.0,0.0,0.0,0.0,0.0 +0,1.049900,0.0,0.0,0.0,0.0,0.0 +0,1.050000,0.0,0.0,0.0,0.0,0.0 +0,1.050100,0.0,0.0,0.0,0.0,0.0 +0,1.050200,0.0,0.0,0.0,0.0,0.0 +0,1.050300,0.0,0.0,0.0,0.0,0.0 +0,1.050400,0.0,0.0,0.0,0.0,0.0 +0,1.050500,0.0,0.0,0.0,0.0,0.0 +0,1.050600,0.0,0.0,0.0,0.0,0.0 +0,1.050700,0.0,0.0,0.0,0.0,0.0 +0,1.050800,0.0,0.0,0.0,0.0,0.0 +0,1.050900,0.0,0.0,0.0,0.0,0.0 +0,1.051000,0.0,0.0,0.0,0.0,0.0 +0,1.051100,0.0,0.0,0.0,0.0,0.0 +0,1.051200,0.0,0.0,0.0,0.0,0.0 +0,1.051300,0.0,0.0,0.0,0.0,0.0 +0,1.051400,0.0,0.0,0.0,0.0,0.0 +0,1.051500,0.0,0.0,0.0,0.0,0.0 +0,1.051600,0.0,0.0,0.0,0.0,0.0 +0,1.051700,0.0,0.0,0.0,0.0,0.0 +0,1.051800,0.0,0.0,0.0,0.0,0.0 +0,1.051900,0.0,0.0,0.0,0.0,0.0 +0,1.052000,0.0,0.0,0.0,0.0,0.0 +0,1.052100,0.0,0.0,0.0,0.0,0.0 +0,1.052200,0.0,0.0,0.0,0.0,0.0 +0,1.052300,0.0,0.0,0.0,0.0,0.0 +0,1.052400,0.0,0.0,0.0,0.0,0.0 +0,1.052500,0.0,0.0,0.0,0.0,0.0 +0,1.052600,0.0,0.0,0.0,0.0,0.0 +0,1.052700,0.0,0.0,0.0,0.0,0.0 +0,1.052800,0.0,0.0,0.0,0.0,0.0 +0,1.052900,0.0,0.0,0.0,0.0,0.0 +0,1.053000,0.0,0.0,0.0,0.0,0.0 +0,1.053100,0.0,0.0,0.0,0.0,0.0 +0,1.053200,0.0,0.0,0.0,0.0,0.0 +0,1.053300,0.0,0.0,0.0,0.0,0.0 +0,1.053400,0.0,0.0,0.0,0.0,0.0 +0,1.053500,0.0,0.0,0.0,0.0,0.0 +0,1.053600,0.0,0.0,0.0,0.0,0.0 +0,1.053700,0.0,0.0,0.0,0.0,0.0 +0,1.053800,0.0,0.0,0.0,0.0,0.0 +0,1.053900,0.0,0.0,0.0,0.0,0.0 +0,1.054000,0.0,0.0,0.0,0.0,0.0 +0,1.054100,0.0,0.0,0.0,0.0,0.0 +0,1.054200,0.0,0.0,0.0,0.0,0.0 +0,1.054300,0.0,0.0,0.0,0.0,0.0 +0,1.054400,0.0,0.0,0.0,0.0,0.0 +0,1.054500,0.0,0.0,0.0,0.0,0.0 +0,1.054600,0.0,0.0,0.0,0.0,0.0 +0,1.054700,0.0,0.0,0.0,0.0,0.0 +0,1.054800,0.0,0.0,0.0,0.0,0.0 +0,1.054900,0.0,0.0,0.0,0.0,0.0 +0,1.055000,0.0,0.0,0.0,0.0,0.0 +0,1.055100,0.0,0.0,0.0,0.0,0.0 +0,1.055200,0.0,0.0,0.0,0.0,0.0 +0,1.055300,0.0,0.0,0.0,0.0,0.0 +0,1.055400,0.0,0.0,0.0,0.0,0.0 +0,1.055500,0.0,0.0,0.0,0.0,0.0 +0,1.055600,0.0,0.0,0.0,0.0,0.0 +0,1.055700,0.0,0.0,0.0,0.0,0.0 +0,1.055800,0.0,0.0,0.0,0.0,0.0 +0,1.055900,0.0,0.0,0.0,0.0,0.0 +0,1.056000,0.0,0.0,0.0,0.0,0.0 +0,1.056100,0.0,0.0,0.0,0.0,0.0 +0,1.056200,0.0,0.0,0.0,0.0,0.0 +0,1.056300,0.0,0.0,0.0,0.0,0.0 +0,1.056400,0.0,0.0,0.0,0.0,0.0 +0,1.056500,0.0,0.0,0.0,0.0,0.0 +0,1.056600,0.0,0.0,0.0,0.0,0.0 +0,1.056700,0.0,0.0,0.0,0.0,0.0 +0,1.056800,0.0,0.0,0.0,0.0,0.0 +0,1.056900,0.0,0.0,0.0,0.0,0.0 +0,1.057000,0.0,0.0,0.0,0.0,0.0 +0,1.057100,0.0,0.0,0.0,0.0,0.0 +0,1.057200,0.0,0.0,0.0,0.0,0.0 +0,1.057300,0.0,0.0,0.0,0.0,0.0 +0,1.057400,0.0,0.0,0.0,0.0,0.0 +0,1.057500,0.0,0.0,0.0,0.0,0.0 +0,1.057600,0.0,0.0,0.0,0.0,0.0 +0,1.057700,0.0,0.0,0.0,0.0,0.0 +0,1.057800,0.0,0.0,0.0,0.0,0.0 +0,1.057900,0.0,0.0,0.0,0.0,0.0 +0,1.058000,0.0,0.0,0.0,0.0,0.0 +0,1.058100,0.0,0.0,0.0,0.0,0.0 +0,1.058200,0.0,0.0,0.0,0.0,0.0 +0,1.058300,0.0,0.0,0.0,0.0,0.0 +0,1.058400,0.0,0.0,0.0,0.0,0.0 +0,1.058500,0.0,0.0,0.0,0.0,0.0 +0,1.058600,0.0,0.0,0.0,0.0,0.0 +0,1.058700,0.0,0.0,0.0,0.0,0.0 +0,1.058800,0.0,0.0,0.0,0.0,0.0 +0,1.058900,0.0,0.0,0.0,0.0,0.0 +0,1.059000,0.0,0.0,0.0,0.0,0.0 +0,1.059100,0.0,0.0,0.0,0.0,0.0 +0,1.059200,0.0,0.0,0.0,0.0,0.0 +0,1.059300,0.0,0.0,0.0,0.0,0.0 +0,1.059400,0.0,0.0,0.0,0.0,0.0 +0,1.059500,0.0,0.0,0.0,0.0,0.0 +0,1.059600,0.0,0.0,0.0,0.0,0.0 +0,1.059700,0.0,0.0,0.0,0.0,0.0 +0,1.059800,0.0,0.0,0.0,0.0,0.0 +0,1.059900,0.0,0.0,0.0,0.0,0.0 +0,1.060000,0.0,0.0,0.0,0.0,0.0 +0,1.060100,0.0,0.0,0.0,0.0,0.0 +1,496.467370,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.060200,0.0,0.0,0.0,0.0,0.0 +0,1.060300,0.0,0.0,0.0,0.0,0.0 +0,1.060400,0.0,0.0,0.0,0.0,0.0 +0,1.060500,0.0,0.0,0.0,0.0,0.0 +0,1.060600,0.0,0.0,0.0,0.0,0.0 +0,1.060700,0.0,0.0,0.0,0.0,0.0 +0,1.060800,0.0,0.0,0.0,0.0,0.0 +0,1.060900,0.0,0.0,0.0,0.0,0.0 +0,1.061000,0.0,0.0,0.0,0.0,0.0 +0,1.061100,0.0,0.0,0.0,0.0,0.0 +0,1.061200,0.0,0.0,0.0,0.0,0.0 +0,1.061300,0.0,0.0,0.0,0.0,0.0 +0,1.061400,0.0,0.0,0.0,0.0,0.0 +0,1.061500,0.0,0.0,0.0,0.0,0.0 +0,1.061600,0.0,0.0,0.0,0.0,0.0 +0,1.061700,0.0,0.0,0.0,0.0,0.0 +0,1.061800,0.0,0.0,0.0,0.0,0.0 +0,1.061900,0.0,0.0,0.0,0.0,0.0 +0,1.062000,0.0,0.0,0.0,0.0,0.0 +0,1.062100,0.0,0.0,0.0,0.0,0.0 +0,1.062200,0.0,0.0,0.0,0.0,0.0 +0,1.062300,0.0,0.0,0.0,0.0,0.0 +0,1.062400,0.0,0.0,0.0,0.0,0.0 +0,1.062500,0.0,0.0,0.0,0.0,0.0 +0,1.062600,0.0,0.0,0.0,0.0,0.0 +0,1.062700,0.0,0.0,0.0,0.0,0.0 +0,1.062800,0.0,0.0,0.0,0.0,0.0 +0,1.062900,0.0,0.0,0.0,0.0,0.0 +0,1.063000,0.0,0.0,0.0,0.0,0.0 +0,1.063100,0.0,0.0,0.0,0.0,0.0 +0,1.063200,0.0,0.0,0.0,0.0,0.0 +0,1.063300,0.0,0.0,0.0,0.0,0.0 +0,1.063400,0.0,0.0,0.0,0.0,0.0 +0,1.063500,0.0,0.0,0.0,0.0,0.0 +0,1.063600,0.0,0.0,0.0,0.0,0.0 +0,1.063700,0.0,0.0,0.0,0.0,0.0 +0,1.063800,0.0,0.0,0.0,0.0,0.0 +0,1.063900,0.0,0.0,0.0,0.0,0.0 +0,1.064000,0.0,0.0,0.0,0.0,0.0 +0,1.064100,0.0,0.0,0.0,0.0,0.0 +0,1.064200,0.0,0.0,0.0,0.0,0.0 +0,1.064300,0.0,0.0,0.0,0.0,0.0 +0,1.064400,0.0,0.0,0.0,0.0,0.0 +0,1.064500,0.0,0.0,0.0,0.0,0.0 +0,1.064600,0.0,0.0,0.0,0.0,0.0 +0,1.064700,0.0,0.0,0.0,0.0,0.0 +0,1.064800,0.0,0.0,0.0,0.0,0.0 +0,1.064900,0.0,0.0,0.0,0.0,0.0 +0,1.065000,0.0,0.0,0.0,0.0,0.0 +0,1.065100,0.0,0.0,0.0,0.0,0.0 +0,1.065200,0.0,0.0,0.0,0.0,0.0 +0,1.065300,0.0,0.0,0.0,0.0,0.0 +0,1.065400,0.0,0.0,0.0,0.0,0.0 +0,1.065500,0.0,0.0,0.0,0.0,0.0 +0,1.065600,0.0,0.0,0.0,0.0,0.0 +0,1.065700,0.0,0.0,0.0,0.0,0.0 +0,1.065800,0.0,0.0,0.0,0.0,0.0 +0,1.065900,0.0,0.0,0.0,0.0,0.0 +0,1.066000,0.0,0.0,0.0,0.0,0.0 +0,1.066100,0.0,0.0,0.0,0.0,0.0 +0,1.066200,0.0,0.0,0.0,0.0,0.0 +0,1.066300,0.0,0.0,0.0,0.0,0.0 +0,1.066400,0.0,0.0,0.0,0.0,0.0 +0,1.066500,0.0,0.0,0.0,0.0,0.0 +0,1.066600,0.0,0.0,0.0,0.0,0.0 +0,1.066700,0.0,0.0,0.0,0.0,0.0 +0,1.066800,0.0,0.0,0.0,0.0,0.0 +0,1.066900,0.0,0.0,0.0,0.0,0.0 +0,1.067000,0.0,0.0,0.0,0.0,0.0 +0,1.067100,0.0,0.0,0.0,0.0,0.0 +0,1.067200,0.0,0.0,0.0,0.0,0.0 +0,1.067300,0.0,0.0,0.0,0.0,0.0 +0,1.067400,0.0,0.0,0.0,0.0,0.0 +0,1.067500,0.0,0.0,0.0,0.0,0.0 +0,1.067600,0.0,0.0,0.0,0.0,0.0 +0,1.067700,0.0,0.0,0.0,0.0,0.0 +0,1.067800,0.0,0.0,0.0,0.0,0.0 +0,1.067900,0.0,0.0,0.0,0.0,0.0 +0,1.068000,0.0,0.0,0.0,0.0,0.0 +0,1.068100,0.0,0.0,0.0,0.0,0.0 +0,1.068200,0.0,0.0,0.0,0.0,0.0 +0,1.068300,0.0,0.0,0.0,0.0,0.0 +0,1.068400,0.0,0.0,0.0,0.0,0.0 +0,1.068500,0.0,0.0,0.0,0.0,0.0 +0,1.068600,0.0,0.0,0.0,0.0,0.0 +0,1.068700,0.0,0.0,0.0,0.0,0.0 +0,1.068800,0.0,0.0,0.0,0.0,0.0 +0,1.068900,0.0,0.0,0.0,0.0,0.0 +0,1.069000,0.0,0.0,0.0,0.0,0.0 +0,1.069100,0.0,0.0,0.0,0.0,0.0 +0,1.069200,0.0,0.0,0.0,0.0,0.0 +0,1.069300,0.0,0.0,0.0,0.0,0.0 +0,1.069400,0.0,0.0,0.0,0.0,0.0 +0,1.069500,0.0,0.0,0.0,0.0,0.0 +0,1.069600,0.0,0.0,0.0,0.0,0.0 +0,1.069700,0.0,0.0,0.0,0.0,0.0 +0,1.069800,0.0,0.0,0.0,0.0,0.0 +0,1.069900,0.0,0.0,0.0,0.0,0.0 +0,1.070000,0.0,0.0,0.0,0.0,0.0 +0,1.070100,0.0,0.0,0.0,0.0,0.0 +0,1.070200,0.0,0.0,0.0,0.0,0.0 +0,1.070300,0.0,0.0,0.0,0.0,0.0 +0,1.070400,0.0,0.0,0.0,0.0,0.0 +0,1.070500,0.0,0.0,0.0,0.0,0.0 +0,1.070600,0.0,0.0,0.0,0.0,0.0 +0,1.070700,0.0,0.0,0.0,0.0,0.0 +0,1.070800,0.0,0.0,0.0,0.0,0.0 +0,1.070900,0.0,0.0,0.0,0.0,0.0 +0,1.071000,0.0,0.0,0.0,0.0,0.0 +0,1.071100,0.0,0.0,0.0,0.0,0.0 +0,1.071200,0.0,0.0,0.0,0.0,0.0 +0,1.071300,0.0,0.0,0.0,0.0,0.0 +0,1.071400,0.0,0.0,0.0,0.0,0.0 +0,1.071500,0.0,0.0,0.0,0.0,0.0 +0,1.071600,0.0,0.0,0.0,0.0,0.0 +0,1.071700,0.0,0.0,0.0,0.0,0.0 +0,1.071800,0.0,0.0,0.0,0.0,0.0 +0,1.071900,0.0,0.0,0.0,0.0,0.0 +0,1.072000,0.0,0.0,0.0,0.0,0.0 +0,1.072100,0.0,0.0,0.0,0.0,0.0 +0,1.072200,0.0,0.0,0.0,0.0,0.0 +0,1.072300,0.0,0.0,0.0,0.0,0.0 +0,1.072400,0.0,0.0,0.0,0.0,0.0 +0,1.072500,0.0,0.0,0.0,0.0,0.0 +0,1.072600,0.0,0.0,0.0,0.0,0.0 +0,1.072700,0.0,0.0,0.0,0.0,0.0 +0,1.072800,0.0,0.0,0.0,0.0,0.0 +0,1.072900,0.0,0.0,0.0,0.0,0.0 +0,1.073000,0.0,0.0,0.0,0.0,0.0 +0,1.073100,0.0,0.0,0.0,0.0,0.0 +0,1.073200,0.0,0.0,0.0,0.0,0.0 +0,1.073300,0.0,0.0,0.0,0.0,0.0 +0,1.073400,0.0,0.0,0.0,0.0,0.0 +0,1.073500,0.0,0.0,0.0,0.0,0.0 +0,1.073600,0.0,0.0,0.0,0.0,0.0 +0,1.073700,0.0,0.0,0.0,0.0,0.0 +0,1.073800,0.0,0.0,0.0,0.0,0.0 +0,1.073900,0.0,0.0,0.0,0.0,0.0 +0,1.074000,0.0,0.0,0.0,0.0,0.0 +0,1.074100,0.0,0.0,0.0,0.0,0.0 +0,1.074200,0.0,0.0,0.0,0.0,0.0 +0,1.074300,0.0,0.0,0.0,0.0,0.0 +0,1.074400,0.0,0.0,0.0,0.0,0.0 +0,1.074500,0.0,0.0,0.0,0.0,0.0 +0,1.074600,0.0,0.0,0.0,0.0,0.0 +0,1.074700,0.0,0.0,0.0,0.0,0.0 +0,1.074800,0.0,0.0,0.0,0.0,0.0 +0,1.074900,0.0,0.0,0.0,0.0,0.0 +0,1.075000,0.0,0.0,0.0,0.0,0.0 +0,1.075100,0.0,0.0,0.0,0.0,0.0 +0,1.075200,0.0,0.0,0.0,0.0,0.0 +0,1.075300,0.0,0.0,0.0,0.0,0.0 +0,1.075400,0.0,0.0,0.0,0.0,0.0 +0,1.075500,0.0,0.0,0.0,0.0,0.0 +0,1.075600,0.0,0.0,0.0,0.0,0.0 +0,1.075700,0.0,0.0,0.0,0.0,0.0 +0,1.075800,0.0,0.0,0.0,0.0,0.0 +0,1.075900,0.0,0.0,0.0,0.0,0.0 +0,1.076000,0.0,0.0,0.0,0.0,0.0 +0,1.076100,0.0,0.0,0.0,0.0,0.0 +0,1.076200,0.0,0.0,0.0,0.0,0.0 +0,1.076300,0.0,0.0,0.0,0.0,0.0 +0,1.076400,0.0,0.0,0.0,0.0,0.0 +0,1.076500,0.0,0.0,0.0,0.0,0.0 +0,1.076600,0.0,0.0,0.0,0.0,0.0 +0,1.076700,0.0,0.0,0.0,0.0,0.0 +0,1.076800,0.0,0.0,0.0,0.0,0.0 +0,1.076900,0.0,0.0,0.0,0.0,0.0 +0,1.077000,0.0,0.0,0.0,0.0,0.0 +0,1.077100,0.0,0.0,0.0,0.0,0.0 +0,1.077200,0.0,0.0,0.0,0.0,0.0 +0,1.077300,0.0,0.0,0.0,0.0,0.0 +0,1.077400,0.0,0.0,0.0,0.0,0.0 +0,1.077500,0.0,0.0,0.0,0.0,0.0 +0,1.077600,0.0,0.0,0.0,0.0,0.0 +0,1.077700,0.0,0.0,0.0,0.0,0.0 +0,1.077800,0.0,0.0,0.0,0.0,0.0 +0,1.077900,0.0,0.0,0.0,0.0,0.0 +0,1.078000,0.0,0.0,0.0,0.0,0.0 +0,1.078100,0.0,0.0,0.0,0.0,0.0 +0,1.078200,0.0,0.0,0.0,0.0,0.0 +0,1.078300,0.0,0.0,0.0,0.0,0.0 +0,1.078400,0.0,0.0,0.0,0.0,0.0 +0,1.078500,0.0,0.0,0.0,0.0,0.0 +0,1.078600,0.0,0.0,0.0,0.0,0.0 +0,1.078700,0.0,0.0,0.0,0.0,0.0 +0,1.078800,0.0,0.0,0.0,0.0,0.0 +0,1.078900,0.0,0.0,0.0,0.0,0.0 +0,1.079000,0.0,0.0,0.0,0.0,0.0 +0,1.079100,0.0,0.0,0.0,0.0,0.0 +0,1.079200,0.0,0.0,0.0,0.0,0.0 +0,1.079300,0.0,0.0,0.0,0.0,0.0 +0,1.079400,0.0,0.0,0.0,0.0,0.0 +0,1.079500,0.0,0.0,0.0,0.0,0.0 +0,1.079600,0.0,0.0,0.0,0.0,0.0 +0,1.079700,0.0,0.0,0.0,0.0,0.0 +0,1.079800,0.0,0.0,0.0,0.0,0.0 +0,1.079900,0.0,0.0,0.0,0.0,0.0 +0,1.080000,0.0,0.0,0.0,0.0,0.0 +0,1.080100,0.0,0.0,0.0,0.0,0.0 +1,525.098729,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.080200,0.0,0.0,0.0,0.0,0.0 +0,1.080300,0.0,0.0,0.0,0.0,0.0 +0,1.080400,0.0,0.0,0.0,0.0,0.0 +0,1.080500,0.0,0.0,0.0,0.0,0.0 +0,1.080600,0.0,0.0,0.0,0.0,0.0 +0,1.080700,0.0,0.0,0.0,0.0,0.0 +0,1.080800,0.0,0.0,0.0,0.0,0.0 +0,1.080900,0.0,0.0,0.0,0.0,0.0 +0,1.081000,0.0,0.0,0.0,0.0,0.0 +0,1.081100,0.0,0.0,0.0,0.0,0.0 +0,1.081200,0.0,0.0,0.0,0.0,0.0 +0,1.081300,0.0,0.0,0.0,0.0,0.0 +0,1.081400,0.0,0.0,0.0,0.0,0.0 +0,1.081500,0.0,0.0,0.0,0.0,0.0 +0,1.081600,0.0,0.0,0.0,0.0,0.0 +0,1.081700,0.0,0.0,0.0,0.0,0.0 +0,1.081800,0.0,0.0,0.0,0.0,0.0 +0,1.081900,0.0,0.0,0.0,0.0,0.0 +0,1.082000,0.0,0.0,0.0,0.0,0.0 +0,1.082100,0.0,0.0,0.0,0.0,0.0 +0,1.082200,0.0,0.0,0.0,0.0,0.0 +0,1.082300,0.0,0.0,0.0,0.0,0.0 +0,1.082400,0.0,0.0,0.0,0.0,0.0 +0,1.082500,0.0,0.0,0.0,0.0,0.0 +0,1.082600,0.0,0.0,0.0,0.0,0.0 +0,1.082700,0.0,0.0,0.0,0.0,0.0 +0,1.082800,0.0,0.0,0.0,0.0,0.0 +0,1.082900,0.0,0.0,0.0,0.0,0.0 +0,1.083000,0.0,0.0,0.0,0.0,0.0 +0,1.083100,0.0,0.0,0.0,0.0,0.0 +0,1.083200,0.0,0.0,0.0,0.0,0.0 +0,1.083300,0.0,0.0,0.0,0.0,0.0 +0,1.083400,0.0,0.0,0.0,0.0,0.0 +0,1.083500,0.0,0.0,0.0,0.0,0.0 +0,1.083600,0.0,0.0,0.0,0.0,0.0 +0,1.083700,0.0,0.0,0.0,0.0,0.0 +0,1.083800,0.0,0.0,0.0,0.0,0.0 +0,1.083900,0.0,0.0,0.0,0.0,0.0 +0,1.084000,0.0,0.0,0.0,0.0,0.0 +0,1.084100,0.0,0.0,0.0,0.0,0.0 +0,1.084200,0.0,0.0,0.0,0.0,0.0 +0,1.084300,0.0,0.0,0.0,0.0,0.0 +0,1.084400,0.0,0.0,0.0,0.0,0.0 +0,1.084500,0.0,0.0,0.0,0.0,0.0 +0,1.084600,0.0,0.0,0.0,0.0,0.0 +0,1.084700,0.0,0.0,0.0,0.0,0.0 +0,1.084800,0.0,0.0,0.0,0.0,0.0 +0,1.084900,0.0,0.0,0.0,0.0,0.0 +0,1.085000,0.0,0.0,0.0,0.0,0.0 +0,1.085100,0.0,0.0,0.0,0.0,0.0 +0,1.085200,0.0,0.0,0.0,0.0,0.0 +0,1.085300,0.0,0.0,0.0,0.0,0.0 +0,1.085400,0.0,0.0,0.0,0.0,0.0 +0,1.085500,0.0,0.0,0.0,0.0,0.0 +0,1.085600,0.0,0.0,0.0,0.0,0.0 +0,1.085700,0.0,0.0,0.0,0.0,0.0 +0,1.085800,0.0,0.0,0.0,0.0,0.0 +0,1.085900,0.0,0.0,0.0,0.0,0.0 +0,1.086000,0.0,0.0,0.0,0.0,0.0 +0,1.086100,0.0,0.0,0.0,0.0,0.0 +0,1.086200,0.0,0.0,0.0,0.0,0.0 +0,1.086300,0.0,0.0,0.0,0.0,0.0 +0,1.086400,0.0,0.0,0.0,0.0,0.0 +0,1.086500,0.0,0.0,0.0,0.0,0.0 +0,1.086600,0.0,0.0,0.0,0.0,0.0 +0,1.086700,0.0,0.0,0.0,0.0,0.0 +0,1.086800,0.0,0.0,0.0,0.0,0.0 +0,1.086900,0.0,0.0,0.0,0.0,0.0 +0,1.087000,0.0,0.0,0.0,0.0,0.0 +0,1.087100,0.0,0.0,0.0,0.0,0.0 +0,1.087200,0.0,0.0,0.0,0.0,0.0 +0,1.087300,0.0,0.0,0.0,0.0,0.0 +0,1.087400,0.0,0.0,0.0,0.0,0.0 +0,1.087500,0.0,0.0,0.0,0.0,0.0 +0,1.087600,0.0,0.0,0.0,0.0,0.0 +0,1.087700,0.0,0.0,0.0,0.0,0.0 +0,1.087800,0.0,0.0,0.0,0.0,0.0 +0,1.087900,0.0,0.0,0.0,0.0,0.0 +0,1.088000,0.0,0.0,0.0,0.0,0.0 +0,1.088100,0.0,0.0,0.0,0.0,0.0 +0,1.088200,0.0,0.0,0.0,0.0,0.0 +0,1.088300,0.0,0.0,0.0,0.0,0.0 +0,1.088400,0.0,0.0,0.0,0.0,0.0 +0,1.088500,0.0,0.0,0.0,0.0,0.0 +0,1.088600,0.0,0.0,0.0,0.0,0.0 +0,1.088700,0.0,0.0,0.0,0.0,0.0 +0,1.088800,0.0,0.0,0.0,0.0,0.0 +0,1.088900,0.0,0.0,0.0,0.0,0.0 +0,1.089000,0.0,0.0,0.0,0.0,0.0 +0,1.089100,0.0,0.0,0.0,0.0,0.0 +0,1.089200,0.0,0.0,0.0,0.0,0.0 +0,1.089300,0.0,0.0,0.0,0.0,0.0 +0,1.089400,0.0,0.0,0.0,0.0,0.0 +0,1.089500,0.0,0.0,0.0,0.0,0.0 +0,1.089600,0.0,0.0,0.0,0.0,0.0 +0,1.089700,0.0,0.0,0.0,0.0,0.0 +0,1.089800,0.0,0.0,0.0,0.0,0.0 +0,1.089900,0.0,0.0,0.0,0.0,0.0 +0,1.090000,0.0,0.0,0.0,0.0,0.0 +0,1.090100,0.0,0.0,0.0,0.0,0.0 +0,1.090200,0.0,0.0,0.0,0.0,0.0 +0,1.090300,0.0,0.0,0.0,0.0,0.0 +0,1.090400,0.0,0.0,0.0,0.0,0.0 +0,1.090500,0.0,0.0,0.0,0.0,0.0 +0,1.090600,0.0,0.0,0.0,0.0,0.0 +0,1.090700,0.0,0.0,0.0,0.0,0.0 +0,1.090800,0.0,0.0,0.0,0.0,0.0 +0,1.090900,0.0,0.0,0.0,0.0,0.0 +0,1.091000,0.0,0.0,0.0,0.0,0.0 +0,1.091100,0.0,0.0,0.0,0.0,0.0 +0,1.091200,0.0,0.0,0.0,0.0,0.0 +0,1.091300,0.0,0.0,0.0,0.0,0.0 +0,1.091400,0.0,0.0,0.0,0.0,0.0 +0,1.091500,0.0,0.0,0.0,0.0,0.0 +0,1.091600,0.0,0.0,0.0,0.0,0.0 +0,1.091700,0.0,0.0,0.0,0.0,0.0 +0,1.091800,0.0,0.0,0.0,0.0,0.0 +0,1.091900,0.0,0.0,0.0,0.0,0.0 +0,1.092000,0.0,0.0,0.0,0.0,0.0 +0,1.092100,0.0,0.0,0.0,0.0,0.0 +0,1.092200,0.0,0.0,0.0,0.0,0.0 +0,1.092300,0.0,0.0,0.0,0.0,0.0 +0,1.092400,0.0,0.0,0.0,0.0,0.0 +0,1.092500,0.0,0.0,0.0,0.0,0.0 +0,1.092600,0.0,0.0,0.0,0.0,0.0 +0,1.092700,0.0,0.0,0.0,0.0,0.0 +0,1.092800,0.0,0.0,0.0,0.0,0.0 +0,1.092900,0.0,0.0,0.0,0.0,0.0 +0,1.093000,0.0,0.0,0.0,0.0,0.0 +0,1.093100,0.0,0.0,0.0,0.0,0.0 +0,1.093200,0.0,0.0,0.0,0.0,0.0 +0,1.093300,0.0,0.0,0.0,0.0,0.0 +0,1.093400,0.0,0.0,0.0,0.0,0.0 +0,1.093500,0.0,0.0,0.0,0.0,0.0 +0,1.093600,0.0,0.0,0.0,0.0,0.0 +0,1.093700,0.0,0.0,0.0,0.0,0.0 +0,1.093800,0.0,0.0,0.0,0.0,0.0 +0,1.093900,0.0,0.0,0.0,0.0,0.0 +0,1.094000,0.0,0.0,0.0,0.0,0.0 +0,1.094100,0.0,0.0,0.0,0.0,0.0 +0,1.094200,0.0,0.0,0.0,0.0,0.0 +0,1.094300,0.0,0.0,0.0,0.0,0.0 +0,1.094400,0.0,0.0,0.0,0.0,0.0 +0,1.094500,0.0,0.0,0.0,0.0,0.0 +0,1.094600,0.0,0.0,0.0,0.0,0.0 +0,1.094700,0.0,0.0,0.0,0.0,0.0 +0,1.094800,0.0,0.0,0.0,0.0,0.0 +0,1.094900,0.0,0.0,0.0,0.0,0.0 +0,1.095000,0.0,0.0,0.0,0.0,0.0 +0,1.095100,0.0,0.0,0.0,0.0,0.0 +0,1.095200,0.0,0.0,0.0,0.0,0.0 +0,1.095300,0.0,0.0,0.0,0.0,0.0 +0,1.095400,0.0,0.0,0.0,0.0,0.0 +0,1.095500,0.0,0.0,0.0,0.0,0.0 +0,1.095600,0.0,0.0,0.0,0.0,0.0 +0,1.095700,0.0,0.0,0.0,0.0,0.0 +0,1.095800,0.0,0.0,0.0,0.0,0.0 +0,1.095900,0.0,0.0,0.0,0.0,0.0 +0,1.096000,0.0,0.0,0.0,0.0,0.0 +0,1.096100,0.0,0.0,0.0,0.0,0.0 +0,1.096200,0.0,0.0,0.0,0.0,0.0 +0,1.096300,0.0,0.0,0.0,0.0,0.0 +0,1.096400,0.0,0.0,0.0,0.0,0.0 +0,1.096500,0.0,0.0,0.0,0.0,0.0 +0,1.096600,0.0,0.0,0.0,0.0,0.0 +0,1.096700,0.0,0.0,0.0,0.0,0.0 +0,1.096800,0.0,0.0,0.0,0.0,0.0 +0,1.096900,0.0,0.0,0.0,0.0,0.0 +0,1.097000,0.0,0.0,0.0,0.0,0.0 +0,1.097100,0.0,0.0,0.0,0.0,0.0 +0,1.097200,0.0,0.0,0.0,0.0,0.0 +0,1.097300,0.0,0.0,0.0,0.0,0.0 +0,1.097400,0.0,0.0,0.0,0.0,0.0 +0,1.097500,0.0,0.0,0.0,0.0,0.0 +0,1.097600,0.0,0.0,0.0,0.0,0.0 +0,1.097700,0.0,0.0,0.0,0.0,0.0 +0,1.097800,0.0,0.0,0.0,0.0,0.0 +0,1.097900,0.0,0.0,0.0,0.0,0.0 +0,1.098000,0.0,0.0,0.0,0.0,0.0 +0,1.098100,0.0,0.0,0.0,0.0,0.0 +0,1.098200,0.0,0.0,0.0,0.0,0.0 +0,1.098300,0.0,0.0,0.0,0.0,0.0 +0,1.098400,0.0,0.0,0.0,0.0,0.0 +0,1.098500,0.0,0.0,0.0,0.0,0.0 +0,1.098600,0.0,0.0,0.0,0.0,0.0 +0,1.098700,0.0,0.0,0.0,0.0,0.0 +0,1.098800,0.0,0.0,0.0,0.0,0.0 +0,1.098900,0.0,0.0,0.0,0.0,0.0 +0,1.099000,0.0,0.0,0.0,0.0,0.0 +0,1.099100,0.0,0.0,0.0,0.0,0.0 +0,1.099200,0.0,0.0,0.0,0.0,0.0 +0,1.099300,0.0,0.0,0.0,0.0,0.0 +0,1.099400,0.0,0.0,0.0,0.0,0.0 +0,1.099500,0.0,0.0,0.0,0.0,0.0 +0,1.099600,0.0,0.0,0.0,0.0,0.0 +0,1.099700,0.0,0.0,0.0,0.0,0.0 +0,1.099800,0.0,0.0,0.0,0.0,0.0 +0,1.099900,0.0,0.0,0.0,0.0,0.0 +0,1.100000,0.0,0.0,0.0,0.0,0.0 +0,1.100100,0.0,0.0,0.0,0.0,0.0 +1,554.810238,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.100200,0.0,0.0,0.0,0.0,0.0 +0,1.100300,0.0,0.0,0.0,0.0,0.0 +0,1.100400,0.0,0.0,0.0,0.0,0.0 +0,1.100500,0.0,0.0,0.0,0.0,0.0 +0,1.100600,0.0,0.0,0.0,0.0,0.0 +0,1.100700,0.0,0.0,0.0,0.0,0.0 +0,1.100800,0.0,0.0,0.0,0.0,0.0 +0,1.100900,0.0,0.0,0.0,0.0,0.0 +0,1.101000,0.0,0.0,0.0,0.0,0.0 +0,1.101100,0.0,0.0,0.0,0.0,0.0 +0,1.101200,0.0,0.0,0.0,0.0,0.0 +0,1.101300,0.0,0.0,0.0,0.0,0.0 +0,1.101400,0.0,0.0,0.0,0.0,0.0 +0,1.101500,0.0,0.0,0.0,0.0,0.0 +0,1.101600,0.0,0.0,0.0,0.0,0.0 +0,1.101700,0.0,0.0,0.0,0.0,0.0 +0,1.101800,0.0,0.0,0.0,0.0,0.0 +0,1.101900,0.0,0.0,0.0,0.0,0.0 +0,1.102000,0.0,0.0,0.0,0.0,0.0 +0,1.102100,0.0,0.0,0.0,0.0,0.0 +0,1.102200,0.0,0.0,0.0,0.0,0.0 +0,1.102300,0.0,0.0,0.0,0.0,0.0 +0,1.102400,0.0,0.0,0.0,0.0,0.0 +0,1.102500,0.0,0.0,0.0,0.0,0.0 +0,1.102600,0.0,0.0,0.0,0.0,0.0 +0,1.102700,0.0,0.0,0.0,0.0,0.0 +0,1.102800,0.0,0.0,0.0,0.0,0.0 +0,1.102900,0.0,0.0,0.0,0.0,0.0 +0,1.103000,0.0,0.0,0.0,0.0,0.0 +0,1.103100,0.0,0.0,0.0,0.0,0.0 +0,1.103200,0.0,0.0,0.0,0.0,0.0 +0,1.103300,0.0,0.0,0.0,0.0,0.0 +0,1.103400,0.0,0.0,0.0,0.0,0.0 +0,1.103500,0.0,0.0,0.0,0.0,0.0 +0,1.103600,0.0,0.0,0.0,0.0,0.0 +0,1.103700,0.0,0.0,0.0,0.0,0.0 +0,1.103800,0.0,0.0,0.0,0.0,0.0 +0,1.103900,0.0,0.0,0.0,0.0,0.0 +0,1.104000,0.0,0.0,0.0,0.0,0.0 +0,1.104100,0.0,0.0,0.0,0.0,0.0 +0,1.104200,0.0,0.0,0.0,0.0,0.0 +0,1.104300,0.0,0.0,0.0,0.0,0.0 +0,1.104400,0.0,0.0,0.0,0.0,0.0 +0,1.104500,0.0,0.0,0.0,0.0,0.0 +0,1.104600,0.0,0.0,0.0,0.0,0.0 +0,1.104700,0.0,0.0,0.0,0.0,0.0 +0,1.104800,0.0,0.0,0.0,0.0,0.0 +0,1.104900,0.0,0.0,0.0,0.0,0.0 +0,1.105000,0.0,0.0,0.0,0.0,0.0 +0,1.105100,0.0,0.0,0.0,0.0,0.0 +0,1.105200,0.0,0.0,0.0,0.0,0.0 +0,1.105300,0.0,0.0,0.0,0.0,0.0 +0,1.105400,0.0,0.0,0.0,0.0,0.0 +0,1.105500,0.0,0.0,0.0,0.0,0.0 +0,1.105600,0.0,0.0,0.0,0.0,0.0 +0,1.105700,0.0,0.0,0.0,0.0,0.0 +0,1.105800,0.0,0.0,0.0,0.0,0.0 +0,1.105900,0.0,0.0,0.0,0.0,0.0 +0,1.106000,0.0,0.0,0.0,0.0,0.0 +0,1.106100,0.0,0.0,0.0,0.0,0.0 +0,1.106200,0.0,0.0,0.0,0.0,0.0 +0,1.106300,0.0,0.0,0.0,0.0,0.0 +0,1.106400,0.0,0.0,0.0,0.0,0.0 +0,1.106500,0.0,0.0,0.0,0.0,0.0 +0,1.106600,0.0,0.0,0.0,0.0,0.0 +0,1.106700,0.0,0.0,0.0,0.0,0.0 +0,1.106800,0.0,0.0,0.0,0.0,0.0 +0,1.106900,0.0,0.0,0.0,0.0,0.0 +0,1.107000,0.0,0.0,0.0,0.0,0.0 +0,1.107100,0.0,0.0,0.0,0.0,0.0 +0,1.107200,0.0,0.0,0.0,0.0,0.0 +0,1.107300,0.0,0.0,0.0,0.0,0.0 +0,1.107400,0.0,0.0,0.0,0.0,0.0 +0,1.107500,0.0,0.0,0.0,0.0,0.0 +0,1.107600,0.0,0.0,0.0,0.0,0.0 +0,1.107700,0.0,0.0,0.0,0.0,0.0 +0,1.107800,0.0,0.0,0.0,0.0,0.0 +0,1.107900,0.0,0.0,0.0,0.0,0.0 +0,1.108000,0.0,0.0,0.0,0.0,0.0 +0,1.108100,0.0,0.0,0.0,0.0,0.0 +0,1.108200,0.0,0.0,0.0,0.0,0.0 +0,1.108300,0.0,0.0,0.0,0.0,0.0 +0,1.108400,0.0,0.0,0.0,0.0,0.0 +0,1.108500,0.0,0.0,0.0,0.0,0.0 +0,1.108600,0.0,0.0,0.0,0.0,0.0 +0,1.108700,0.0,0.0,0.0,0.0,0.0 +0,1.108800,0.0,0.0,0.0,0.0,0.0 +0,1.108900,0.0,0.0,0.0,0.0,0.0 +0,1.109000,0.0,0.0,0.0,0.0,0.0 +0,1.109100,0.0,0.0,0.0,0.0,0.0 +0,1.109200,0.0,0.0,0.0,0.0,0.0 +0,1.109300,0.0,0.0,0.0,0.0,0.0 +0,1.109400,0.0,0.0,0.0,0.0,0.0 +0,1.109500,0.0,0.0,0.0,0.0,0.0 +0,1.109600,0.0,0.0,0.0,0.0,0.0 +0,1.109700,0.0,0.0,0.0,0.0,0.0 +0,1.109800,0.0,0.0,0.0,0.0,0.0 +0,1.109900,0.0,0.0,0.0,0.0,0.0 +0,1.110000,0.0,0.0,0.0,0.0,0.0 +0,1.110100,0.0,0.0,0.0,0.0,0.0 +0,1.110200,0.0,0.0,0.0,0.0,0.0 +0,1.110300,0.0,0.0,0.0,0.0,0.0 +0,1.110400,0.0,0.0,0.0,0.0,0.0 +0,1.110500,0.0,0.0,0.0,0.0,0.0 +0,1.110600,0.0,0.0,0.0,0.0,0.0 +0,1.110700,0.0,0.0,0.0,0.0,0.0 +0,1.110800,0.0,0.0,0.0,0.0,0.0 +0,1.110900,0.0,0.0,0.0,0.0,0.0 +0,1.111000,0.0,0.0,0.0,0.0,0.0 +0,1.111100,0.0,0.0,0.0,0.0,0.0 +0,1.111200,0.0,0.0,0.0,0.0,0.0 +0,1.111300,0.0,0.0,0.0,0.0,0.0 +0,1.111400,0.0,0.0,0.0,0.0,0.0 +0,1.111500,0.0,0.0,0.0,0.0,0.0 +0,1.111600,0.0,0.0,0.0,0.0,0.0 +0,1.111700,0.0,0.0,0.0,0.0,0.0 +0,1.111800,0.0,0.0,0.0,0.0,0.0 +0,1.111900,0.0,0.0,0.0,0.0,0.0 +0,1.112000,0.0,0.0,0.0,0.0,0.0 +0,1.112100,0.0,0.0,0.0,0.0,0.0 +0,1.112200,0.0,0.0,0.0,0.0,0.0 +0,1.112300,0.0,0.0,0.0,0.0,0.0 +0,1.112400,0.0,0.0,0.0,0.0,0.0 +0,1.112500,0.0,0.0,0.0,0.0,0.0 +0,1.112600,0.0,0.0,0.0,0.0,0.0 +0,1.112700,0.0,0.0,0.0,0.0,0.0 +0,1.112800,0.0,0.0,0.0,0.0,0.0 +0,1.112900,0.0,0.0,0.0,0.0,0.0 +0,1.113000,0.0,0.0,0.0,0.0,0.0 +0,1.113100,0.0,0.0,0.0,0.0,0.0 +0,1.113200,0.0,0.0,0.0,0.0,0.0 +0,1.113300,0.0,0.0,0.0,0.0,0.0 +0,1.113400,0.0,0.0,0.0,0.0,0.0 +0,1.113500,0.0,0.0,0.0,0.0,0.0 +0,1.113600,0.0,0.0,0.0,0.0,0.0 +0,1.113700,0.0,0.0,0.0,0.0,0.0 +0,1.113800,0.0,0.0,0.0,0.0,0.0 +0,1.113900,0.0,0.0,0.0,0.0,0.0 +0,1.114000,0.0,0.0,0.0,0.0,0.0 +0,1.114100,0.0,0.0,0.0,0.0,0.0 +0,1.114200,0.0,0.0,0.0,0.0,0.0 +0,1.114300,0.0,0.0,0.0,0.0,0.0 +0,1.114400,0.0,0.0,0.0,0.0,0.0 +0,1.114500,0.0,0.0,0.0,0.0,0.0 +0,1.114600,0.0,0.0,0.0,0.0,0.0 +0,1.114700,0.0,0.0,0.0,0.0,0.0 +0,1.114800,0.0,0.0,0.0,0.0,0.0 +0,1.114900,0.0,0.0,0.0,0.0,0.0 +0,1.115000,0.0,0.0,0.0,0.0,0.0 +0,1.115100,0.0,0.0,0.0,0.0,0.0 +0,1.115200,0.0,0.0,0.0,0.0,0.0 +0,1.115300,0.0,0.0,0.0,0.0,0.0 +0,1.115400,0.0,0.0,0.0,0.0,0.0 +0,1.115500,0.0,0.0,0.0,0.0,0.0 +0,1.115600,0.0,0.0,0.0,0.0,0.0 +0,1.115700,0.0,0.0,0.0,0.0,0.0 +0,1.115800,0.0,0.0,0.0,0.0,0.0 +0,1.115900,0.0,0.0,0.0,0.0,0.0 +0,1.116000,0.0,0.0,0.0,0.0,0.0 +0,1.116100,0.0,0.0,0.0,0.0,0.0 +0,1.116200,0.0,0.0,0.0,0.0,0.0 +0,1.116300,0.0,0.0,0.0,0.0,0.0 +0,1.116400,0.0,0.0,0.0,0.0,0.0 +0,1.116500,0.0,0.0,0.0,0.0,0.0 +0,1.116600,0.0,0.0,0.0,0.0,0.0 +0,1.116700,0.0,0.0,0.0,0.0,0.0 +0,1.116800,0.0,0.0,0.0,0.0,0.0 +0,1.116900,0.0,0.0,0.0,0.0,0.0 +0,1.117000,0.0,0.0,0.0,0.0,0.0 +0,1.117100,0.0,0.0,0.0,0.0,0.0 +0,1.117200,0.0,0.0,0.0,0.0,0.0 +0,1.117300,0.0,0.0,0.0,0.0,0.0 +0,1.117400,0.0,0.0,0.0,0.0,0.0 +0,1.117500,0.0,0.0,0.0,0.0,0.0 +0,1.117600,0.0,0.0,0.0,0.0,0.0 +0,1.117700,0.0,0.0,0.0,0.0,0.0 +0,1.117800,0.0,0.0,0.0,0.0,0.0 +0,1.117900,0.0,0.0,0.0,0.0,0.0 +0,1.118000,0.0,0.0,0.0,0.0,0.0 +0,1.118100,0.0,0.0,0.0,0.0,0.0 +0,1.118200,0.0,0.0,0.0,0.0,0.0 +0,1.118300,0.0,0.0,0.0,0.0,0.0 +0,1.118400,0.0,0.0,0.0,0.0,0.0 +0,1.118500,0.0,0.0,0.0,0.0,0.0 +0,1.118600,0.0,0.0,0.0,0.0,0.0 +0,1.118700,0.0,0.0,0.0,0.0,0.0 +0,1.118800,0.0,0.0,0.0,0.0,0.0 +0,1.118900,0.0,0.0,0.0,0.0,0.0 +0,1.119000,0.0,0.0,0.0,0.0,0.0 +0,1.119100,0.0,0.0,0.0,0.0,0.0 +0,1.119200,0.0,0.0,0.0,0.0,0.0 +0,1.119300,0.0,0.0,0.0,0.0,0.0 +0,1.119400,0.0,0.0,0.0,0.0,0.0 +0,1.119500,0.0,0.0,0.0,0.0,0.0 +0,1.119600,0.0,0.0,0.0,0.0,0.0 +0,1.119700,0.0,0.0,0.0,0.0,0.0 +0,1.119800,0.0,0.0,0.0,0.0,0.0 +0,1.119900,0.0,0.0,0.0,0.0,0.0 +0,1.120000,0.0,0.0,0.0,0.0,0.0 +0,1.120100,0.0,0.0,0.0,0.0,0.0 +1,585.621897,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.120200,0.0,0.0,0.0,0.0,0.0 +0,1.120300,0.0,0.0,0.0,0.0,0.0 +0,1.120400,0.0,0.0,0.0,0.0,0.0 +0,1.120500,0.0,0.0,0.0,0.0,0.0 +0,1.120600,0.0,0.0,0.0,0.0,0.0 +0,1.120700,0.0,0.0,0.0,0.0,0.0 +0,1.120800,0.0,0.0,0.0,0.0,0.0 +0,1.120900,0.0,0.0,0.0,0.0,0.0 +0,1.121000,0.0,0.0,0.0,0.0,0.0 +0,1.121100,0.0,0.0,0.0,0.0,0.0 +0,1.121200,0.0,0.0,0.0,0.0,0.0 +0,1.121300,0.0,0.0,0.0,0.0,0.0 +0,1.121400,0.0,0.0,0.0,0.0,0.0 +0,1.121500,0.0,0.0,0.0,0.0,0.0 +0,1.121600,0.0,0.0,0.0,0.0,0.0 +0,1.121700,0.0,0.0,0.0,0.0,0.0 +0,1.121800,0.0,0.0,0.0,0.0,0.0 +0,1.121900,0.0,0.0,0.0,0.0,0.0 +0,1.122000,0.0,0.0,0.0,0.0,0.0 +0,1.122100,0.0,0.0,0.0,0.0,0.0 +0,1.122200,0.0,0.0,0.0,0.0,0.0 +0,1.122300,0.0,0.0,0.0,0.0,0.0 +0,1.122400,0.0,0.0,0.0,0.0,0.0 +0,1.122500,0.0,0.0,0.0,0.0,0.0 +0,1.122600,0.0,0.0,0.0,0.0,0.0 +0,1.122700,0.0,0.0,0.0,0.0,0.0 +0,1.122800,0.0,0.0,0.0,0.0,0.0 +0,1.122900,0.0,0.0,0.0,0.0,0.0 +0,1.123000,0.0,0.0,0.0,0.0,0.0 +0,1.123100,0.0,0.0,0.0,0.0,0.0 +0,1.123200,0.0,0.0,0.0,0.0,0.0 +0,1.123300,0.0,0.0,0.0,0.0,0.0 +0,1.123400,0.0,0.0,0.0,0.0,0.0 +0,1.123500,0.0,0.0,0.0,0.0,0.0 +0,1.123600,0.0,0.0,0.0,0.0,0.0 +0,1.123700,0.0,0.0,0.0,0.0,0.0 +0,1.123800,0.0,0.0,0.0,0.0,0.0 +0,1.123900,0.0,0.0,0.0,0.0,0.0 +0,1.124000,0.0,0.0,0.0,0.0,0.0 +0,1.124100,0.0,0.0,0.0,0.0,0.0 +0,1.124200,0.0,0.0,0.0,0.0,0.0 +0,1.124300,0.0,0.0,0.0,0.0,0.0 +0,1.124400,0.0,0.0,0.0,0.0,0.0 +0,1.124500,0.0,0.0,0.0,0.0,0.0 +0,1.124600,0.0,0.0,0.0,0.0,0.0 +0,1.124700,0.0,0.0,0.0,0.0,0.0 +0,1.124800,0.0,0.0,0.0,0.0,0.0 +0,1.124900,0.0,0.0,0.0,0.0,0.0 +0,1.125000,0.0,0.0,0.0,0.0,0.0 +0,1.125100,0.0,0.0,0.0,0.0,0.0 +0,1.125200,0.0,0.0,0.0,0.0,0.0 +0,1.125300,0.0,0.0,0.0,0.0,0.0 +0,1.125400,0.0,0.0,0.0,0.0,0.0 +0,1.125500,0.0,0.0,0.0,0.0,0.0 +0,1.125600,0.0,0.0,0.0,0.0,0.0 +0,1.125700,0.0,0.0,0.0,0.0,0.0 +0,1.125800,0.0,0.0,0.0,0.0,0.0 +0,1.125900,0.0,0.0,0.0,0.0,0.0 +0,1.126000,0.0,0.0,0.0,0.0,0.0 +0,1.126100,0.0,0.0,0.0,0.0,0.0 +0,1.126200,0.0,0.0,0.0,0.0,0.0 +0,1.126300,0.0,0.0,0.0,0.0,0.0 +0,1.126400,0.0,0.0,0.0,0.0,0.0 +0,1.126500,0.0,0.0,0.0,0.0,0.0 +0,1.126600,0.0,0.0,0.0,0.0,0.0 +0,1.126700,0.0,0.0,0.0,0.0,0.0 +0,1.126800,0.0,0.0,0.0,0.0,0.0 +0,1.126900,0.0,0.0,0.0,0.0,0.0 +0,1.127000,0.0,0.0,0.0,0.0,0.0 +0,1.127100,0.0,0.0,0.0,0.0,0.0 +0,1.127200,0.0,0.0,0.0,0.0,0.0 +0,1.127300,0.0,0.0,0.0,0.0,0.0 +0,1.127400,0.0,0.0,0.0,0.0,0.0 +0,1.127500,0.0,0.0,0.0,0.0,0.0 +0,1.127600,0.0,0.0,0.0,0.0,0.0 +0,1.127700,0.0,0.0,0.0,0.0,0.0 +0,1.127800,0.0,0.0,0.0,0.0,0.0 +0,1.127900,0.0,0.0,0.0,0.0,0.0 +0,1.128000,0.0,0.0,0.0,0.0,0.0 +0,1.128100,0.0,0.0,0.0,0.0,0.0 +0,1.128200,0.0,0.0,0.0,0.0,0.0 +0,1.128300,0.0,0.0,0.0,0.0,0.0 +0,1.128400,0.0,0.0,0.0,0.0,0.0 +0,1.128500,0.0,0.0,0.0,0.0,0.0 +0,1.128600,0.0,0.0,0.0,0.0,0.0 +0,1.128700,0.0,0.0,0.0,0.0,0.0 +0,1.128800,0.0,0.0,0.0,0.0,0.0 +0,1.128900,0.0,0.0,0.0,0.0,0.0 +0,1.129000,0.0,0.0,0.0,0.0,0.0 +0,1.129100,0.0,0.0,0.0,0.0,0.0 +0,1.129200,0.0,0.0,0.0,0.0,0.0 +0,1.129300,0.0,0.0,0.0,0.0,0.0 +0,1.129400,0.0,0.0,0.0,0.0,0.0 +0,1.129500,0.0,0.0,0.0,0.0,0.0 +0,1.129600,0.0,0.0,0.0,0.0,0.0 +0,1.129700,0.0,0.0,0.0,0.0,0.0 +0,1.129800,0.0,0.0,0.0,0.0,0.0 +0,1.129900,0.0,0.0,0.0,0.0,0.0 +0,1.130000,0.0,0.0,0.0,0.0,0.0 +0,1.130100,0.0,0.0,0.0,0.0,0.0 +0,1.130200,0.0,0.0,0.0,0.0,0.0 +0,1.130300,0.0,0.0,0.0,0.0,0.0 +0,1.130400,0.0,0.0,0.0,0.0,0.0 +0,1.130500,0.0,0.0,0.0,0.0,0.0 +0,1.130600,0.0,0.0,0.0,0.0,0.0 +0,1.130700,0.0,0.0,0.0,0.0,0.0 +0,1.130800,0.0,0.0,0.0,0.0,0.0 +0,1.130900,0.0,0.0,0.0,0.0,0.0 +0,1.131000,0.0,0.0,0.0,0.0,0.0 +0,1.131100,0.0,0.0,0.0,0.0,0.0 +0,1.131200,0.0,0.0,0.0,0.0,0.0 +0,1.131300,0.0,0.0,0.0,0.0,0.0 +0,1.131400,0.0,0.0,0.0,0.0,0.0 +0,1.131500,0.0,0.0,0.0,0.0,0.0 +0,1.131600,0.0,0.0,0.0,0.0,0.0 +0,1.131700,0.0,0.0,0.0,0.0,0.0 +0,1.131800,0.0,0.0,0.0,0.0,0.0 +0,1.131900,0.0,0.0,0.0,0.0,0.0 +0,1.132000,0.0,0.0,0.0,0.0,0.0 +0,1.132100,0.0,0.0,0.0,0.0,0.0 +0,1.132200,0.0,0.0,0.0,0.0,0.0 +0,1.132300,0.0,0.0,0.0,0.0,0.0 +0,1.132400,0.0,0.0,0.0,0.0,0.0 +0,1.132500,0.0,0.0,0.0,0.0,0.0 +0,1.132600,0.0,0.0,0.0,0.0,0.0 +0,1.132700,0.0,0.0,0.0,0.0,0.0 +0,1.132800,0.0,0.0,0.0,0.0,0.0 +0,1.132900,0.0,0.0,0.0,0.0,0.0 +0,1.133000,0.0,0.0,0.0,0.0,0.0 +0,1.133100,0.0,0.0,0.0,0.0,0.0 +0,1.133200,0.0,0.0,0.0,0.0,0.0 +0,1.133300,0.0,0.0,0.0,0.0,0.0 +0,1.133400,0.0,0.0,0.0,0.0,0.0 +0,1.133500,0.0,0.0,0.0,0.0,0.0 +0,1.133600,0.0,0.0,0.0,0.0,0.0 +0,1.133700,0.0,0.0,0.0,0.0,0.0 +0,1.133800,0.0,0.0,0.0,0.0,0.0 +0,1.133900,0.0,0.0,0.0,0.0,0.0 +0,1.134000,0.0,0.0,0.0,0.0,0.0 +0,1.134100,0.0,0.0,0.0,0.0,0.0 +0,1.134200,0.0,0.0,0.0,0.0,0.0 +0,1.134300,0.0,0.0,0.0,0.0,0.0 +0,1.134400,0.0,0.0,0.0,0.0,0.0 +0,1.134500,0.0,0.0,0.0,0.0,0.0 +0,1.134600,0.0,0.0,0.0,0.0,0.0 +0,1.134700,0.0,0.0,0.0,0.0,0.0 +0,1.134800,0.0,0.0,0.0,0.0,0.0 +0,1.134900,0.0,0.0,0.0,0.0,0.0 +0,1.135000,0.0,0.0,0.0,0.0,0.0 +0,1.135100,0.0,0.0,0.0,0.0,0.0 +0,1.135200,0.0,0.0,0.0,0.0,0.0 +0,1.135300,0.0,0.0,0.0,0.0,0.0 +0,1.135400,0.0,0.0,0.0,0.0,0.0 +0,1.135500,0.0,0.0,0.0,0.0,0.0 +0,1.135600,0.0,0.0,0.0,0.0,0.0 +0,1.135700,0.0,0.0,0.0,0.0,0.0 +0,1.135800,0.0,0.0,0.0,0.0,0.0 +0,1.135900,0.0,0.0,0.0,0.0,0.0 +0,1.136000,0.0,0.0,0.0,0.0,0.0 +0,1.136100,0.0,0.0,0.0,0.0,0.0 +0,1.136200,0.0,0.0,0.0,0.0,0.0 +0,1.136300,0.0,0.0,0.0,0.0,0.0 +0,1.136400,0.0,0.0,0.0,0.0,0.0 +0,1.136500,0.0,0.0,0.0,0.0,0.0 +0,1.136600,0.0,0.0,0.0,0.0,0.0 +0,1.136700,0.0,0.0,0.0,0.0,0.0 +0,1.136800,0.0,0.0,0.0,0.0,0.0 +0,1.136900,0.0,0.0,0.0,0.0,0.0 +0,1.137000,0.0,0.0,0.0,0.0,0.0 +0,1.137100,0.0,0.0,0.0,0.0,0.0 +0,1.137200,0.0,0.0,0.0,0.0,0.0 +0,1.137300,0.0,0.0,0.0,0.0,0.0 +0,1.137400,0.0,0.0,0.0,0.0,0.0 +0,1.137500,0.0,0.0,0.0,0.0,0.0 +0,1.137600,0.0,0.0,0.0,0.0,0.0 +0,1.137700,0.0,0.0,0.0,0.0,0.0 +0,1.137800,0.0,0.0,0.0,0.0,0.0 +0,1.137900,0.0,0.0,0.0,0.0,0.0 +0,1.138000,0.0,0.0,0.0,0.0,0.0 +0,1.138100,0.0,0.0,0.0,0.0,0.0 +0,1.138200,0.0,0.0,0.0,0.0,0.0 +0,1.138300,0.0,0.0,0.0,0.0,0.0 +0,1.138400,0.0,0.0,0.0,0.0,0.0 +0,1.138500,0.0,0.0,0.0,0.0,0.0 +0,1.138600,0.0,0.0,0.0,0.0,0.0 +0,1.138700,0.0,0.0,0.0,0.0,0.0 +0,1.138800,0.0,0.0,0.0,0.0,0.0 +0,1.138900,0.0,0.0,0.0,0.0,0.0 +0,1.139000,0.0,0.0,0.0,0.0,0.0 +0,1.139100,0.0,0.0,0.0,0.0,0.0 +0,1.139200,0.0,0.0,0.0,0.0,0.0 +0,1.139300,0.0,0.0,0.0,0.0,0.0 +0,1.139400,0.0,0.0,0.0,0.0,0.0 +0,1.139500,0.0,0.0,0.0,0.0,0.0 +0,1.139600,0.0,0.0,0.0,0.0,0.0 +0,1.139700,0.0,0.0,0.0,0.0,0.0 +0,1.139800,0.0,0.0,0.0,0.0,0.0 +0,1.139900,0.0,0.0,0.0,0.0,0.0 +0,1.140000,0.0,0.0,0.0,0.0,0.0 +0,1.140100,0.0,0.0,0.0,0.0,0.0 +1,617.553706,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.140200,0.0,0.0,0.0,0.0,0.0 +0,1.140300,0.0,0.0,0.0,0.0,0.0 +0,1.140400,0.0,0.0,0.0,0.0,0.0 +0,1.140500,0.0,0.0,0.0,0.0,0.0 +0,1.140600,0.0,0.0,0.0,0.0,0.0 +0,1.140700,0.0,0.0,0.0,0.0,0.0 +0,1.140800,0.0,0.0,0.0,0.0,0.0 +0,1.140900,0.0,0.0,0.0,0.0,0.0 +0,1.141000,0.0,0.0,0.0,0.0,0.0 +0,1.141100,0.0,0.0,0.0,0.0,0.0 +0,1.141200,0.0,0.0,0.0,0.0,0.0 +0,1.141300,0.0,0.0,0.0,0.0,0.0 +0,1.141400,0.0,0.0,0.0,0.0,0.0 +0,1.141500,0.0,0.0,0.0,0.0,0.0 +0,1.141600,0.0,0.0,0.0,0.0,0.0 +0,1.141700,0.0,0.0,0.0,0.0,0.0 +0,1.141800,0.0,0.0,0.0,0.0,0.0 +0,1.141900,0.0,0.0,0.0,0.0,0.0 +0,1.142000,0.0,0.0,0.0,0.0,0.0 +0,1.142100,0.0,0.0,0.0,0.0,0.0 +0,1.142200,0.0,0.0,0.0,0.0,0.0 +0,1.142300,0.0,0.0,0.0,0.0,0.0 +0,1.142400,0.0,0.0,0.0,0.0,0.0 +0,1.142500,0.0,0.0,0.0,0.0,0.0 +0,1.142600,0.0,0.0,0.0,0.0,0.0 +0,1.142700,0.0,0.0,0.0,0.0,0.0 +0,1.142800,0.0,0.0,0.0,0.0,0.0 +0,1.142900,0.0,0.0,0.0,0.0,0.0 +0,1.143000,0.0,0.0,0.0,0.0,0.0 +0,1.143100,0.0,0.0,0.0,0.0,0.0 +0,1.143200,0.0,0.0,0.0,0.0,0.0 +0,1.143300,0.0,0.0,0.0,0.0,0.0 +0,1.143400,0.0,0.0,0.0,0.0,0.0 +0,1.143500,0.0,0.0,0.0,0.0,0.0 +0,1.143600,0.0,0.0,0.0,0.0,0.0 +0,1.143700,0.0,0.0,0.0,0.0,0.0 +0,1.143800,0.0,0.0,0.0,0.0,0.0 +0,1.143900,0.0,0.0,0.0,0.0,0.0 +0,1.144000,0.0,0.0,0.0,0.0,0.0 +0,1.144100,0.0,0.0,0.0,0.0,0.0 +0,1.144200,0.0,0.0,0.0,0.0,0.0 +0,1.144300,0.0,0.0,0.0,0.0,0.0 +0,1.144400,0.0,0.0,0.0,0.0,0.0 +0,1.144500,0.0,0.0,0.0,0.0,0.0 +0,1.144600,0.0,0.0,0.0,0.0,0.0 +0,1.144700,0.0,0.0,0.0,0.0,0.0 +0,1.144800,0.0,0.0,0.0,0.0,0.0 +0,1.144900,0.0,0.0,0.0,0.0,0.0 +0,1.145000,0.0,0.0,0.0,0.0,0.0 +0,1.145100,0.0,0.0,0.0,0.0,0.0 +0,1.145200,0.0,0.0,0.0,0.0,0.0 +0,1.145300,0.0,0.0,0.0,0.0,0.0 +0,1.145400,0.0,0.0,0.0,0.0,0.0 +0,1.145500,0.0,0.0,0.0,0.0,0.0 +0,1.145600,0.0,0.0,0.0,0.0,0.0 +0,1.145700,0.0,0.0,0.0,0.0,0.0 +0,1.145800,0.0,0.0,0.0,0.0,0.0 +0,1.145900,0.0,0.0,0.0,0.0,0.0 +0,1.146000,0.0,0.0,0.0,0.0,0.0 +0,1.146100,0.0,0.0,0.0,0.0,0.0 +0,1.146200,0.0,0.0,0.0,0.0,0.0 +0,1.146300,0.0,0.0,0.0,0.0,0.0 +0,1.146400,0.0,0.0,0.0,0.0,0.0 +0,1.146500,0.0,0.0,0.0,0.0,0.0 +0,1.146600,0.0,0.0,0.0,0.0,0.0 +0,1.146700,0.0,0.0,0.0,0.0,0.0 +0,1.146800,0.0,0.0,0.0,0.0,0.0 +0,1.146900,0.0,0.0,0.0,0.0,0.0 +0,1.147000,0.0,0.0,0.0,0.0,0.0 +0,1.147100,0.0,0.0,0.0,0.0,0.0 +0,1.147200,0.0,0.0,0.0,0.0,0.0 +0,1.147300,0.0,0.0,0.0,0.0,0.0 +0,1.147400,0.0,0.0,0.0,0.0,0.0 +0,1.147500,0.0,0.0,0.0,0.0,0.0 +0,1.147600,0.0,0.0,0.0,0.0,0.0 +0,1.147700,0.0,0.0,0.0,0.0,0.0 +0,1.147800,0.0,0.0,0.0,0.0,0.0 +0,1.147900,0.0,0.0,0.0,0.0,0.0 +0,1.148000,0.0,0.0,0.0,0.0,0.0 +0,1.148100,0.0,0.0,0.0,0.0,0.0 +0,1.148200,0.0,0.0,0.0,0.0,0.0 +0,1.148300,0.0,0.0,0.0,0.0,0.0 +0,1.148400,0.0,0.0,0.0,0.0,0.0 +0,1.148500,0.0,0.0,0.0,0.0,0.0 +0,1.148600,0.0,0.0,0.0,0.0,0.0 +0,1.148700,0.0,0.0,0.0,0.0,0.0 +0,1.148800,0.0,0.0,0.0,0.0,0.0 +0,1.148900,0.0,0.0,0.0,0.0,0.0 +0,1.149000,0.0,0.0,0.0,0.0,0.0 +0,1.149100,0.0,0.0,0.0,0.0,0.0 +0,1.149200,0.0,0.0,0.0,0.0,0.0 +0,1.149300,0.0,0.0,0.0,0.0,0.0 +0,1.149400,0.0,0.0,0.0,0.0,0.0 +0,1.149500,0.0,0.0,0.0,0.0,0.0 +0,1.149600,0.0,0.0,0.0,0.0,0.0 +0,1.149700,0.0,0.0,0.0,0.0,0.0 +0,1.149800,0.0,0.0,0.0,0.0,0.0 +0,1.149900,0.0,0.0,0.0,0.0,0.0 +0,1.150000,0.0,0.0,0.0,0.0,0.0 +0,1.150100,0.0,0.0,0.0,0.0,0.0 +0,1.150200,0.0,0.0,0.0,0.0,0.0 +0,1.150300,0.0,0.0,0.0,0.0,0.0 +0,1.150400,0.0,0.0,0.0,0.0,0.0 +0,1.150500,0.0,0.0,0.0,0.0,0.0 +0,1.150600,0.0,0.0,0.0,0.0,0.0 +0,1.150700,0.0,0.0,0.0,0.0,0.0 +0,1.150800,0.0,0.0,0.0,0.0,0.0 +0,1.150900,0.0,0.0,0.0,0.0,0.0 +0,1.151000,0.0,0.0,0.0,0.0,0.0 +0,1.151100,0.0,0.0,0.0,0.0,0.0 +0,1.151200,0.0,0.0,0.0,0.0,0.0 +0,1.151300,0.0,0.0,0.0,0.0,0.0 +0,1.151400,0.0,0.0,0.0,0.0,0.0 +0,1.151500,0.0,0.0,0.0,0.0,0.0 +0,1.151600,0.0,0.0,0.0,0.0,0.0 +0,1.151700,0.0,0.0,0.0,0.0,0.0 +0,1.151800,0.0,0.0,0.0,0.0,0.0 +0,1.151900,0.0,0.0,0.0,0.0,0.0 +0,1.152000,0.0,0.0,0.0,0.0,0.0 +0,1.152100,0.0,0.0,0.0,0.0,0.0 +0,1.152200,0.0,0.0,0.0,0.0,0.0 +0,1.152300,0.0,0.0,0.0,0.0,0.0 +0,1.152400,0.0,0.0,0.0,0.0,0.0 +0,1.152500,0.0,0.0,0.0,0.0,0.0 +0,1.152600,0.0,0.0,0.0,0.0,0.0 +0,1.152700,0.0,0.0,0.0,0.0,0.0 +0,1.152800,0.0,0.0,0.0,0.0,0.0 +0,1.152900,0.0,0.0,0.0,0.0,0.0 +0,1.153000,0.0,0.0,0.0,0.0,0.0 +0,1.153100,0.0,0.0,0.0,0.0,0.0 +0,1.153200,0.0,0.0,0.0,0.0,0.0 +0,1.153300,0.0,0.0,0.0,0.0,0.0 +0,1.153400,0.0,0.0,0.0,0.0,0.0 +0,1.153500,0.0,0.0,0.0,0.0,0.0 +0,1.153600,0.0,0.0,0.0,0.0,0.0 +0,1.153700,0.0,0.0,0.0,0.0,0.0 +0,1.153800,0.0,0.0,0.0,0.0,0.0 +0,1.153900,0.0,0.0,0.0,0.0,0.0 +0,1.154000,0.0,0.0,0.0,0.0,0.0 +0,1.154100,0.0,0.0,0.0,0.0,0.0 +0,1.154200,0.0,0.0,0.0,0.0,0.0 +0,1.154300,0.0,0.0,0.0,0.0,0.0 +0,1.154400,0.0,0.0,0.0,0.0,0.0 +0,1.154500,0.0,0.0,0.0,0.0,0.0 +0,1.154600,0.0,0.0,0.0,0.0,0.0 +0,1.154700,0.0,0.0,0.0,0.0,0.0 +0,1.154800,0.0,0.0,0.0,0.0,0.0 +0,1.154900,0.0,0.0,0.0,0.0,0.0 +0,1.155000,0.0,0.0,0.0,0.0,0.0 +0,1.155100,0.0,0.0,0.0,0.0,0.0 +0,1.155200,0.0,0.0,0.0,0.0,0.0 +0,1.155300,0.0,0.0,0.0,0.0,0.0 +0,1.155400,0.0,0.0,0.0,0.0,0.0 +0,1.155500,0.0,0.0,0.0,0.0,0.0 +0,1.155600,0.0,0.0,0.0,0.0,0.0 +0,1.155700,0.0,0.0,0.0,0.0,0.0 +0,1.155800,0.0,0.0,0.0,0.0,0.0 +0,1.155900,0.0,0.0,0.0,0.0,0.0 +0,1.156000,0.0,0.0,0.0,0.0,0.0 +0,1.156100,0.0,0.0,0.0,0.0,0.0 +0,1.156200,0.0,0.0,0.0,0.0,0.0 +0,1.156300,0.0,0.0,0.0,0.0,0.0 +0,1.156400,0.0,0.0,0.0,0.0,0.0 +0,1.156500,0.0,0.0,0.0,0.0,0.0 +0,1.156600,0.0,0.0,0.0,0.0,0.0 +0,1.156700,0.0,0.0,0.0,0.0,0.0 +0,1.156800,0.0,0.0,0.0,0.0,0.0 +0,1.156900,0.0,0.0,0.0,0.0,0.0 +0,1.157000,0.0,0.0,0.0,0.0,0.0 +0,1.157100,0.0,0.0,0.0,0.0,0.0 +0,1.157200,0.0,0.0,0.0,0.0,0.0 +0,1.157300,0.0,0.0,0.0,0.0,0.0 +0,1.157400,0.0,0.0,0.0,0.0,0.0 +0,1.157500,0.0,0.0,0.0,0.0,0.0 +0,1.157600,0.0,0.0,0.0,0.0,0.0 +0,1.157700,0.0,0.0,0.0,0.0,0.0 +0,1.157800,0.0,0.0,0.0,0.0,0.0 +0,1.157900,0.0,0.0,0.0,0.0,0.0 +0,1.158000,0.0,0.0,0.0,0.0,0.0 +0,1.158100,0.0,0.0,0.0,0.0,0.0 +0,1.158200,0.0,0.0,0.0,0.0,0.0 +0,1.158300,0.0,0.0,0.0,0.0,0.0 +0,1.158400,0.0,0.0,0.0,0.0,0.0 +0,1.158500,0.0,0.0,0.0,0.0,0.0 +0,1.158600,0.0,0.0,0.0,0.0,0.0 +0,1.158700,0.0,0.0,0.0,0.0,0.0 +0,1.158800,0.0,0.0,0.0,0.0,0.0 +0,1.158900,0.0,0.0,0.0,0.0,0.0 +0,1.159000,0.0,0.0,0.0,0.0,0.0 +0,1.159100,0.0,0.0,0.0,0.0,0.0 +0,1.159200,0.0,0.0,0.0,0.0,0.0 +0,1.159300,0.0,0.0,0.0,0.0,0.0 +0,1.159400,0.0,0.0,0.0,0.0,0.0 +0,1.159500,0.0,0.0,0.0,0.0,0.0 +0,1.159600,0.0,0.0,0.0,0.0,0.0 +0,1.159700,0.0,0.0,0.0,0.0,0.0 +0,1.159800,0.0,0.0,0.0,0.0,0.0 +0,1.159900,0.0,0.0,0.0,0.0,0.0 +0,1.160000,0.0,0.0,0.0,0.0,0.0 +0,1.160100,0.0,0.0,0.0,0.0,0.0 +1,650.625665,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.160200,0.0,0.0,0.0,0.0,0.0 +0,1.160300,0.0,0.0,0.0,0.0,0.0 +0,1.160400,0.0,0.0,0.0,0.0,0.0 +0,1.160500,0.0,0.0,0.0,0.0,0.0 +0,1.160600,0.0,0.0,0.0,0.0,0.0 +0,1.160700,0.0,0.0,0.0,0.0,0.0 +0,1.160800,0.0,0.0,0.0,0.0,0.0 +0,1.160900,0.0,0.0,0.0,0.0,0.0 +0,1.161000,0.0,0.0,0.0,0.0,0.0 +0,1.161100,0.0,0.0,0.0,0.0,0.0 +0,1.161200,0.0,0.0,0.0,0.0,0.0 +0,1.161300,0.0,0.0,0.0,0.0,0.0 +0,1.161400,0.0,0.0,0.0,0.0,0.0 +0,1.161500,0.0,0.0,0.0,0.0,0.0 +0,1.161600,0.0,0.0,0.0,0.0,0.0 +0,1.161700,0.0,0.0,0.0,0.0,0.0 +0,1.161800,0.0,0.0,0.0,0.0,0.0 +0,1.161900,0.0,0.0,0.0,0.0,0.0 +0,1.162000,0.0,0.0,0.0,0.0,0.0 +0,1.162100,0.0,0.0,0.0,0.0,0.0 +0,1.162200,0.0,0.0,0.0,0.0,0.0 +0,1.162300,0.0,0.0,0.0,0.0,0.0 +0,1.162400,0.0,0.0,0.0,0.0,0.0 +0,1.162500,0.0,0.0,0.0,0.0,0.0 +0,1.162600,0.0,0.0,0.0,0.0,0.0 +0,1.162700,0.0,0.0,0.0,0.0,0.0 +0,1.162800,0.0,0.0,0.0,0.0,0.0 +0,1.162900,0.0,0.0,0.0,0.0,0.0 +0,1.163000,0.0,0.0,0.0,0.0,0.0 +0,1.163100,0.0,0.0,0.0,0.0,0.0 +0,1.163200,0.0,0.0,0.0,0.0,0.0 +0,1.163300,0.0,0.0,0.0,0.0,0.0 +0,1.163400,0.0,0.0,0.0,0.0,0.0 +0,1.163500,0.0,0.0,0.0,0.0,0.0 +0,1.163600,0.0,0.0,0.0,0.0,0.0 +0,1.163700,0.0,0.0,0.0,0.0,0.0 +0,1.163800,0.0,0.0,0.0,0.0,0.0 +0,1.163900,0.0,0.0,0.0,0.0,0.0 +0,1.164000,0.0,0.0,0.0,0.0,0.0 +0,1.164100,0.0,0.0,0.0,0.0,0.0 +0,1.164200,0.0,0.0,0.0,0.0,0.0 +0,1.164300,0.0,0.0,0.0,0.0,0.0 +0,1.164400,0.0,0.0,0.0,0.0,0.0 +0,1.164500,0.0,0.0,0.0,0.0,0.0 +0,1.164600,0.0,0.0,0.0,0.0,0.0 +0,1.164700,0.0,0.0,0.0,0.0,0.0 +0,1.164800,0.0,0.0,0.0,0.0,0.0 +0,1.164900,0.0,0.0,0.0,0.0,0.0 +0,1.165000,0.0,0.0,0.0,0.0,0.0 +0,1.165100,0.0,0.0,0.0,0.0,0.0 +0,1.165200,0.0,0.0,0.0,0.0,0.0 +0,1.165300,0.0,0.0,0.0,0.0,0.0 +0,1.165400,0.0,0.0,0.0,0.0,0.0 +0,1.165500,0.0,0.0,0.0,0.0,0.0 +0,1.165600,0.0,0.0,0.0,0.0,0.0 +0,1.165700,0.0,0.0,0.0,0.0,0.0 +0,1.165800,0.0,0.0,0.0,0.0,0.0 +0,1.165900,0.0,0.0,0.0,0.0,0.0 +0,1.166000,0.0,0.0,0.0,0.0,0.0 +0,1.166100,0.0,0.0,0.0,0.0,0.0 +0,1.166200,0.0,0.0,0.0,0.0,0.0 +0,1.166300,0.0,0.0,0.0,0.0,0.0 +0,1.166400,0.0,0.0,0.0,0.0,0.0 +0,1.166500,0.0,0.0,0.0,0.0,0.0 +0,1.166600,0.0,0.0,0.0,0.0,0.0 +0,1.166700,0.0,0.0,0.0,0.0,0.0 +0,1.166800,0.0,0.0,0.0,0.0,0.0 +0,1.166900,0.0,0.0,0.0,0.0,0.0 +0,1.167000,0.0,0.0,0.0,0.0,0.0 +0,1.167100,0.0,0.0,0.0,0.0,0.0 +0,1.167200,0.0,0.0,0.0,0.0,0.0 +0,1.167300,0.0,0.0,0.0,0.0,0.0 +0,1.167400,0.0,0.0,0.0,0.0,0.0 +0,1.167500,0.0,0.0,0.0,0.0,0.0 +0,1.167600,0.0,0.0,0.0,0.0,0.0 +0,1.167700,0.0,0.0,0.0,0.0,0.0 +0,1.167800,0.0,0.0,0.0,0.0,0.0 +0,1.167900,0.0,0.0,0.0,0.0,0.0 +0,1.168000,0.0,0.0,0.0,0.0,0.0 +0,1.168100,0.0,0.0,0.0,0.0,0.0 +0,1.168200,0.0,0.0,0.0,0.0,0.0 +0,1.168300,0.0,0.0,0.0,0.0,0.0 +0,1.168400,0.0,0.0,0.0,0.0,0.0 +0,1.168500,0.0,0.0,0.0,0.0,0.0 +0,1.168600,0.0,0.0,0.0,0.0,0.0 +0,1.168700,0.0,0.0,0.0,0.0,0.0 +0,1.168800,0.0,0.0,0.0,0.0,0.0 +0,1.168900,0.0,0.0,0.0,0.0,0.0 +0,1.169000,0.0,0.0,0.0,0.0,0.0 +0,1.169100,0.0,0.0,0.0,0.0,0.0 +0,1.169200,0.0,0.0,0.0,0.0,0.0 +0,1.169300,0.0,0.0,0.0,0.0,0.0 +0,1.169400,0.0,0.0,0.0,0.0,0.0 +0,1.169500,0.0,0.0,0.0,0.0,0.0 +0,1.169600,0.0,0.0,0.0,0.0,0.0 +0,1.169700,0.0,0.0,0.0,0.0,0.0 +0,1.169800,0.0,0.0,0.0,0.0,0.0 +0,1.169900,0.0,0.0,0.0,0.0,0.0 +0,1.170000,0.0,0.0,0.0,0.0,0.0 +0,1.170100,0.0,0.0,0.0,0.0,0.0 +0,1.170200,0.0,0.0,0.0,0.0,0.0 +0,1.170300,0.0,0.0,0.0,0.0,0.0 +0,1.170400,0.0,0.0,0.0,0.0,0.0 +0,1.170500,0.0,0.0,0.0,0.0,0.0 +0,1.170600,0.0,0.0,0.0,0.0,0.0 +0,1.170700,0.0,0.0,0.0,0.0,0.0 +0,1.170800,0.0,0.0,0.0,0.0,0.0 +0,1.170900,0.0,0.0,0.0,0.0,0.0 +0,1.171000,0.0,0.0,0.0,0.0,0.0 +0,1.171100,0.0,0.0,0.0,0.0,0.0 +0,1.171200,0.0,0.0,0.0,0.0,0.0 +0,1.171300,0.0,0.0,0.0,0.0,0.0 +0,1.171400,0.0,0.0,0.0,0.0,0.0 +0,1.171500,0.0,0.0,0.0,0.0,0.0 +0,1.171600,0.0,0.0,0.0,0.0,0.0 +0,1.171700,0.0,0.0,0.0,0.0,0.0 +0,1.171800,0.0,0.0,0.0,0.0,0.0 +0,1.171900,0.0,0.0,0.0,0.0,0.0 +0,1.172000,0.0,0.0,0.0,0.0,0.0 +0,1.172100,0.0,0.0,0.0,0.0,0.0 +0,1.172200,0.0,0.0,0.0,0.0,0.0 +0,1.172300,0.0,0.0,0.0,0.0,0.0 +0,1.172400,0.0,0.0,0.0,0.0,0.0 +0,1.172500,0.0,0.0,0.0,0.0,0.0 +0,1.172600,0.0,0.0,0.0,0.0,0.0 +0,1.172700,0.0,0.0,0.0,0.0,0.0 +0,1.172800,0.0,0.0,0.0,0.0,0.0 +0,1.172900,0.0,0.0,0.0,0.0,0.0 +0,1.173000,0.0,0.0,0.0,0.0,0.0 +0,1.173100,0.0,0.0,0.0,0.0,0.0 +0,1.173200,0.0,0.0,0.0,0.0,0.0 +0,1.173300,0.0,0.0,0.0,0.0,0.0 +0,1.173400,0.0,0.0,0.0,0.0,0.0 +0,1.173500,0.0,0.0,0.0,0.0,0.0 +0,1.173600,0.0,0.0,0.0,0.0,0.0 +0,1.173700,0.0,0.0,0.0,0.0,0.0 +0,1.173800,0.0,0.0,0.0,0.0,0.0 +0,1.173900,0.0,0.0,0.0,0.0,0.0 +0,1.174000,0.0,0.0,0.0,0.0,0.0 +0,1.174100,0.0,0.0,0.0,0.0,0.0 +0,1.174200,0.0,0.0,0.0,0.0,0.0 +0,1.174300,0.0,0.0,0.0,0.0,0.0 +0,1.174400,0.0,0.0,0.0,0.0,0.0 +0,1.174500,0.0,0.0,0.0,0.0,0.0 +0,1.174600,0.0,0.0,0.0,0.0,0.0 +0,1.174700,0.0,0.0,0.0,0.0,0.0 +0,1.174800,0.0,0.0,0.0,0.0,0.0 +0,1.174900,0.0,0.0,0.0,0.0,0.0 +0,1.175000,0.0,0.0,0.0,0.0,0.0 +0,1.175100,0.0,0.0,0.0,0.0,0.0 +0,1.175200,0.0,0.0,0.0,0.0,0.0 +0,1.175300,0.0,0.0,0.0,0.0,0.0 +0,1.175400,0.0,0.0,0.0,0.0,0.0 +0,1.175500,0.0,0.0,0.0,0.0,0.0 +0,1.175600,0.0,0.0,0.0,0.0,0.0 +0,1.175700,0.0,0.0,0.0,0.0,0.0 +0,1.175800,0.0,0.0,0.0,0.0,0.0 +0,1.175900,0.0,0.0,0.0,0.0,0.0 +0,1.176000,0.0,0.0,0.0,0.0,0.0 +0,1.176100,0.0,0.0,0.0,0.0,0.0 +0,1.176200,0.0,0.0,0.0,0.0,0.0 +0,1.176300,0.0,0.0,0.0,0.0,0.0 +0,1.176400,0.0,0.0,0.0,0.0,0.0 +0,1.176500,0.0,0.0,0.0,0.0,0.0 +0,1.176600,0.0,0.0,0.0,0.0,0.0 +0,1.176700,0.0,0.0,0.0,0.0,0.0 +0,1.176800,0.0,0.0,0.0,0.0,0.0 +0,1.176900,0.0,0.0,0.0,0.0,0.0 +0,1.177000,0.0,0.0,0.0,0.0,0.0 +0,1.177100,0.0,0.0,0.0,0.0,0.0 +0,1.177200,0.0,0.0,0.0,0.0,0.0 +0,1.177300,0.0,0.0,0.0,0.0,0.0 +0,1.177400,0.0,0.0,0.0,0.0,0.0 +0,1.177500,0.0,0.0,0.0,0.0,0.0 +0,1.177600,0.0,0.0,0.0,0.0,0.0 +0,1.177700,0.0,0.0,0.0,0.0,0.0 +0,1.177800,0.0,0.0,0.0,0.0,0.0 +0,1.177900,0.0,0.0,0.0,0.0,0.0 +0,1.178000,0.0,0.0,0.0,0.0,0.0 +0,1.178100,0.0,0.0,0.0,0.0,0.0 +0,1.178200,0.0,0.0,0.0,0.0,0.0 +0,1.178300,0.0,0.0,0.0,0.0,0.0 +0,1.178400,0.0,0.0,0.0,0.0,0.0 +0,1.178500,0.0,0.0,0.0,0.0,0.0 +0,1.178600,0.0,0.0,0.0,0.0,0.0 +0,1.178700,0.0,0.0,0.0,0.0,0.0 +0,1.178800,0.0,0.0,0.0,0.0,0.0 +0,1.178900,0.0,0.0,0.0,0.0,0.0 +0,1.179000,0.0,0.0,0.0,0.0,0.0 +0,1.179100,0.0,0.0,0.0,0.0,0.0 +0,1.179200,0.0,0.0,0.0,0.0,0.0 +0,1.179300,0.0,0.0,0.0,0.0,0.0 +0,1.179400,0.0,0.0,0.0,0.0,0.0 +0,1.179500,0.0,0.0,0.0,0.0,0.0 +0,1.179600,0.0,0.0,0.0,0.0,0.0 +0,1.179700,0.0,0.0,0.0,0.0,0.0 +0,1.179800,0.0,0.0,0.0,0.0,0.0 +0,1.179900,0.0,0.0,0.0,0.0,0.0 +0,1.180000,0.0,0.0,0.0,0.0,0.0 +0,1.180100,0.0,0.0,0.0,0.0,0.0 +1,684.857774,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.180200,0.0,0.0,0.0,0.0,0.0 +0,1.180300,0.0,0.0,0.0,0.0,0.0 +0,1.180400,0.0,0.0,0.0,0.0,0.0 +0,1.180500,0.0,0.0,0.0,0.0,0.0 +0,1.180600,0.0,0.0,0.0,0.0,0.0 +0,1.180700,0.0,0.0,0.0,0.0,0.0 +0,1.180800,0.0,0.0,0.0,0.0,0.0 +0,1.180900,0.0,0.0,0.0,0.0,0.0 +0,1.181000,0.0,0.0,0.0,0.0,0.0 +0,1.181100,0.0,0.0,0.0,0.0,0.0 +0,1.181200,0.0,0.0,0.0,0.0,0.0 +0,1.181300,0.0,0.0,0.0,0.0,0.0 +0,1.181400,0.0,0.0,0.0,0.0,0.0 +0,1.181500,0.0,0.0,0.0,0.0,0.0 +0,1.181600,0.0,0.0,0.0,0.0,0.0 +0,1.181700,0.0,0.0,0.0,0.0,0.0 +0,1.181800,0.0,0.0,0.0,0.0,0.0 +0,1.181900,0.0,0.0,0.0,0.0,0.0 +0,1.182000,0.0,0.0,0.0,0.0,0.0 +0,1.182100,0.0,0.0,0.0,0.0,0.0 +0,1.182200,0.0,0.0,0.0,0.0,0.0 +0,1.182300,0.0,0.0,0.0,0.0,0.0 +0,1.182400,0.0,0.0,0.0,0.0,0.0 +0,1.182500,0.0,0.0,0.0,0.0,0.0 +0,1.182600,0.0,0.0,0.0,0.0,0.0 +0,1.182700,0.0,0.0,0.0,0.0,0.0 +0,1.182800,0.0,0.0,0.0,0.0,0.0 +0,1.182900,0.0,0.0,0.0,0.0,0.0 +0,1.183000,0.0,0.0,0.0,0.0,0.0 +0,1.183100,0.0,0.0,0.0,0.0,0.0 +0,1.183200,0.0,0.0,0.0,0.0,0.0 +0,1.183300,0.0,0.0,0.0,0.0,0.0 +0,1.183400,0.0,0.0,0.0,0.0,0.0 +0,1.183500,0.0,0.0,0.0,0.0,0.0 +0,1.183600,0.0,0.0,0.0,0.0,0.0 +0,1.183700,0.0,0.0,0.0,0.0,0.0 +0,1.183800,0.0,0.0,0.0,0.0,0.0 +0,1.183900,0.0,0.0,0.0,0.0,0.0 +0,1.184000,0.0,0.0,0.0,0.0,0.0 +0,1.184100,0.0,0.0,0.0,0.0,0.0 +0,1.184200,0.0,0.0,0.0,0.0,0.0 +0,1.184300,0.0,0.0,0.0,0.0,0.0 +0,1.184400,0.0,0.0,0.0,0.0,0.0 +0,1.184500,0.0,0.0,0.0,0.0,0.0 +0,1.184600,0.0,0.0,0.0,0.0,0.0 +0,1.184700,0.0,0.0,0.0,0.0,0.0 +0,1.184800,0.0,0.0,0.0,0.0,0.0 +0,1.184900,0.0,0.0,0.0,0.0,0.0 +0,1.185000,0.0,0.0,0.0,0.0,0.0 +0,1.185100,0.0,0.0,0.0,0.0,0.0 +0,1.185200,0.0,0.0,0.0,0.0,0.0 +0,1.185300,0.0,0.0,0.0,0.0,0.0 +0,1.185400,0.0,0.0,0.0,0.0,0.0 +0,1.185500,0.0,0.0,0.0,0.0,0.0 +0,1.185600,0.0,0.0,0.0,0.0,0.0 +0,1.185700,0.0,0.0,0.0,0.0,0.0 +0,1.185800,0.0,0.0,0.0,0.0,0.0 +0,1.185900,0.0,0.0,0.0,0.0,0.0 +0,1.186000,0.0,0.0,0.0,0.0,0.0 +0,1.186100,0.0,0.0,0.0,0.0,0.0 +0,1.186200,0.0,0.0,0.0,0.0,0.0 +0,1.186300,0.0,0.0,0.0,0.0,0.0 +0,1.186400,0.0,0.0,0.0,0.0,0.0 +0,1.186500,0.0,0.0,0.0,0.0,0.0 +0,1.186600,0.0,0.0,0.0,0.0,0.0 +0,1.186700,0.0,0.0,0.0,0.0,0.0 +0,1.186800,0.0,0.0,0.0,0.0,0.0 +0,1.186900,0.0,0.0,0.0,0.0,0.0 +0,1.187000,0.0,0.0,0.0,0.0,0.0 +0,1.187100,0.0,0.0,0.0,0.0,0.0 +0,1.187200,0.0,0.0,0.0,0.0,0.0 +0,1.187300,0.0,0.0,0.0,0.0,0.0 +0,1.187400,0.0,0.0,0.0,0.0,0.0 +0,1.187500,0.0,0.0,0.0,0.0,0.0 +0,1.187600,0.0,0.0,0.0,0.0,0.0 +0,1.187700,0.0,0.0,0.0,0.0,0.0 +0,1.187800,0.0,0.0,0.0,0.0,0.0 +0,1.187900,0.0,0.0,0.0,0.0,0.0 +0,1.188000,0.0,0.0,0.0,0.0,0.0 +0,1.188100,0.0,0.0,0.0,0.0,0.0 +0,1.188200,0.0,0.0,0.0,0.0,0.0 +0,1.188300,0.0,0.0,0.0,0.0,0.0 +0,1.188400,0.0,0.0,0.0,0.0,0.0 +0,1.188500,0.0,0.0,0.0,0.0,0.0 +0,1.188600,0.0,0.0,0.0,0.0,0.0 +0,1.188700,0.0,0.0,0.0,0.0,0.0 +0,1.188800,0.0,0.0,0.0,0.0,0.0 +0,1.188900,0.0,0.0,0.0,0.0,0.0 +0,1.189000,0.0,0.0,0.0,0.0,0.0 +0,1.189100,0.0,0.0,0.0,0.0,0.0 +0,1.189200,0.0,0.0,0.0,0.0,0.0 +0,1.189300,0.0,0.0,0.0,0.0,0.0 +0,1.189400,0.0,0.0,0.0,0.0,0.0 +0,1.189500,0.0,0.0,0.0,0.0,0.0 +0,1.189600,0.0,0.0,0.0,0.0,0.0 +0,1.189700,0.0,0.0,0.0,0.0,0.0 +0,1.189800,0.0,0.0,0.0,0.0,0.0 +0,1.189900,0.0,0.0,0.0,0.0,0.0 +0,1.190000,0.0,0.0,0.0,0.0,0.0 +0,1.190100,0.0,0.0,0.0,0.0,0.0 +0,1.190200,0.0,0.0,0.0,0.0,0.0 +0,1.190300,0.0,0.0,0.0,0.0,0.0 +0,1.190400,0.0,0.0,0.0,0.0,0.0 +0,1.190500,0.0,0.0,0.0,0.0,0.0 +0,1.190600,0.0,0.0,0.0,0.0,0.0 +0,1.190700,0.0,0.0,0.0,0.0,0.0 +0,1.190800,0.0,0.0,0.0,0.0,0.0 +0,1.190900,0.0,0.0,0.0,0.0,0.0 +0,1.191000,0.0,0.0,0.0,0.0,0.0 +0,1.191100,0.0,0.0,0.0,0.0,0.0 +0,1.191200,0.0,0.0,0.0,0.0,0.0 +0,1.191300,0.0,0.0,0.0,0.0,0.0 +0,1.191400,0.0,0.0,0.0,0.0,0.0 +0,1.191500,0.0,0.0,0.0,0.0,0.0 +0,1.191600,0.0,0.0,0.0,0.0,0.0 +0,1.191700,0.0,0.0,0.0,0.0,0.0 +0,1.191800,0.0,0.0,0.0,0.0,0.0 +0,1.191900,0.0,0.0,0.0,0.0,0.0 +0,1.192000,0.0,0.0,0.0,0.0,0.0 +0,1.192100,0.0,0.0,0.0,0.0,0.0 +0,1.192200,0.0,0.0,0.0,0.0,0.0 +0,1.192300,0.0,0.0,0.0,0.0,0.0 +0,1.192400,0.0,0.0,0.0,0.0,0.0 +0,1.192500,0.0,0.0,0.0,0.0,0.0 +0,1.192600,0.0,0.0,0.0,0.0,0.0 +0,1.192700,0.0,0.0,0.0,0.0,0.0 +0,1.192800,0.0,0.0,0.0,0.0,0.0 +0,1.192900,0.0,0.0,0.0,0.0,0.0 +0,1.193000,0.0,0.0,0.0,0.0,0.0 +0,1.193100,0.0,0.0,0.0,0.0,0.0 +0,1.193200,0.0,0.0,0.0,0.0,0.0 +0,1.193300,0.0,0.0,0.0,0.0,0.0 +0,1.193400,0.0,0.0,0.0,0.0,0.0 +0,1.193500,0.0,0.0,0.0,0.0,0.0 +0,1.193600,0.0,0.0,0.0,0.0,0.0 +0,1.193700,0.0,0.0,0.0,0.0,0.0 +0,1.193800,0.0,0.0,0.0,0.0,0.0 +0,1.193900,0.0,0.0,0.0,0.0,0.0 +0,1.194000,0.0,0.0,0.0,0.0,0.0 +0,1.194100,0.0,0.0,0.0,0.0,0.0 +0,1.194200,0.0,0.0,0.0,0.0,0.0 +0,1.194300,0.0,0.0,0.0,0.0,0.0 +0,1.194400,0.0,0.0,0.0,0.0,0.0 +0,1.194500,0.0,0.0,0.0,0.0,0.0 +0,1.194600,0.0,0.0,0.0,0.0,0.0 +0,1.194700,0.0,0.0,0.0,0.0,0.0 +0,1.194800,0.0,0.0,0.0,0.0,0.0 +0,1.194900,0.0,0.0,0.0,0.0,0.0 +0,1.195000,0.0,0.0,0.0,0.0,0.0 +0,1.195100,0.0,0.0,0.0,0.0,0.0 +0,1.195200,0.0,0.0,0.0,0.0,0.0 +0,1.195300,0.0,0.0,0.0,0.0,0.0 +0,1.195400,0.0,0.0,0.0,0.0,0.0 +0,1.195500,0.0,0.0,0.0,0.0,0.0 +0,1.195600,0.0,0.0,0.0,0.0,0.0 +0,1.195700,0.0,0.0,0.0,0.0,0.0 +0,1.195800,0.0,0.0,0.0,0.0,0.0 +0,1.195900,0.0,0.0,0.0,0.0,0.0 +0,1.196000,0.0,0.0,0.0,0.0,0.0 +0,1.196100,0.0,0.0,0.0,0.0,0.0 +0,1.196200,0.0,0.0,0.0,0.0,0.0 +0,1.196300,0.0,0.0,0.0,0.0,0.0 +0,1.196400,0.0,0.0,0.0,0.0,0.0 +0,1.196500,0.0,0.0,0.0,0.0,0.0 +0,1.196600,0.0,0.0,0.0,0.0,0.0 +0,1.196700,0.0,0.0,0.0,0.0,0.0 +0,1.196800,0.0,0.0,0.0,0.0,0.0 +0,1.196900,0.0,0.0,0.0,0.0,0.0 +0,1.197000,0.0,0.0,0.0,0.0,0.0 +0,1.197100,0.0,0.0,0.0,0.0,0.0 +0,1.197200,0.0,0.0,0.0,0.0,0.0 +0,1.197300,0.0,0.0,0.0,0.0,0.0 +0,1.197400,0.0,0.0,0.0,0.0,0.0 +0,1.197500,0.0,0.0,0.0,0.0,0.0 +0,1.197600,0.0,0.0,0.0,0.0,0.0 +0,1.197700,0.0,0.0,0.0,0.0,0.0 +0,1.197800,0.0,0.0,0.0,0.0,0.0 +0,1.197900,0.0,0.0,0.0,0.0,0.0 +0,1.198000,0.0,0.0,0.0,0.0,0.0 +0,1.198100,0.0,0.0,0.0,0.0,0.0 +0,1.198200,0.0,0.0,0.0,0.0,0.0 +0,1.198300,0.0,0.0,0.0,0.0,0.0 +0,1.198400,0.0,0.0,0.0,0.0,0.0 +0,1.198500,0.0,0.0,0.0,0.0,0.0 +0,1.198600,0.0,0.0,0.0,0.0,0.0 +0,1.198700,0.0,0.0,0.0,0.0,0.0 +0,1.198800,0.0,0.0,0.0,0.0,0.0 +0,1.198900,0.0,0.0,0.0,0.0,0.0 +0,1.199000,0.0,0.0,0.0,0.0,0.0 +0,1.199100,0.0,0.0,0.0,0.0,0.0 +0,1.199200,0.0,0.0,0.0,0.0,0.0 +0,1.199300,0.0,0.0,0.0,0.0,0.0 +0,1.199400,0.0,0.0,0.0,0.0,0.0 +0,1.199500,0.0,0.0,0.0,0.0,0.0 +0,1.199600,0.0,0.0,0.0,0.0,0.0 +0,1.199700,0.0,0.0,0.0,0.0,0.0 +0,1.199800,0.0,0.0,0.0,0.0,0.0 +0,1.199900,0.0,0.0,0.0,0.0,0.0 +0,1.200000,0.0,0.0,0.0,0.0,0.0 +0,1.200100,0.0,0.0,0.0,0.0,0.0 +1,720.270033,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.200200,0.0,0.0,0.0,0.0,0.0 +0,1.200300,0.0,0.0,0.0,0.0,0.0 +0,1.200400,0.0,0.0,0.0,0.0,0.0 +0,1.200500,0.0,0.0,0.0,0.0,0.0 +0,1.200600,0.0,0.0,0.0,0.0,0.0 +0,1.200700,0.0,0.0,0.0,0.0,0.0 +0,1.200800,0.0,0.0,0.0,0.0,0.0 +0,1.200900,0.0,0.0,0.0,0.0,0.0 +0,1.201000,0.0,0.0,0.0,0.0,0.0 +0,1.201100,0.0,0.0,0.0,0.0,0.0 +0,1.201200,0.0,0.0,0.0,0.0,0.0 +0,1.201300,0.0,0.0,0.0,0.0,0.0 +0,1.201400,0.0,0.0,0.0,0.0,0.0 +0,1.201500,0.0,0.0,0.0,0.0,0.0 +0,1.201600,0.0,0.0,0.0,0.0,0.0 +0,1.201700,0.0,0.0,0.0,0.0,0.0 +0,1.201800,0.0,0.0,0.0,0.0,0.0 +0,1.201900,0.0,0.0,0.0,0.0,0.0 +0,1.202000,0.0,0.0,0.0,0.0,0.0 +0,1.202100,0.0,0.0,0.0,0.0,0.0 +0,1.202200,0.0,0.0,0.0,0.0,0.0 +0,1.202300,0.0,0.0,0.0,0.0,0.0 +0,1.202400,0.0,0.0,0.0,0.0,0.0 +0,1.202500,0.0,0.0,0.0,0.0,0.0 +0,1.202600,0.0,0.0,0.0,0.0,0.0 +0,1.202700,0.0,0.0,0.0,0.0,0.0 +0,1.202800,0.0,0.0,0.0,0.0,0.0 +0,1.202900,0.0,0.0,0.0,0.0,0.0 +0,1.203000,0.0,0.0,0.0,0.0,0.0 +0,1.203100,0.0,0.0,0.0,0.0,0.0 +0,1.203200,0.0,0.0,0.0,0.0,0.0 +0,1.203300,0.0,0.0,0.0,0.0,0.0 +0,1.203400,0.0,0.0,0.0,0.0,0.0 +0,1.203500,0.0,0.0,0.0,0.0,0.0 +0,1.203600,0.0,0.0,0.0,0.0,0.0 +0,1.203700,0.0,0.0,0.0,0.0,0.0 +0,1.203800,0.0,0.0,0.0,0.0,0.0 +0,1.203900,0.0,0.0,0.0,0.0,0.0 +0,1.204000,0.0,0.0,0.0,0.0,0.0 +0,1.204100,0.0,0.0,0.0,0.0,0.0 +0,1.204200,0.0,0.0,0.0,0.0,0.0 +0,1.204300,0.0,0.0,0.0,0.0,0.0 +0,1.204400,0.0,0.0,0.0,0.0,0.0 +0,1.204500,0.0,0.0,0.0,0.0,0.0 +0,1.204600,0.0,0.0,0.0,0.0,0.0 +0,1.204700,0.0,0.0,0.0,0.0,0.0 +0,1.204800,0.0,0.0,0.0,0.0,0.0 +0,1.204900,0.0,0.0,0.0,0.0,0.0 +0,1.205000,0.0,0.0,0.0,0.0,0.0 +0,1.205100,0.0,0.0,0.0,0.0,0.0 +0,1.205200,0.0,0.0,0.0,0.0,0.0 +0,1.205300,0.0,0.0,0.0,0.0,0.0 +0,1.205400,0.0,0.0,0.0,0.0,0.0 +0,1.205500,0.0,0.0,0.0,0.0,0.0 +0,1.205600,0.0,0.0,0.0,0.0,0.0 +0,1.205700,0.0,0.0,0.0,0.0,0.0 +0,1.205800,0.0,0.0,0.0,0.0,0.0 +0,1.205900,0.0,0.0,0.0,0.0,0.0 +0,1.206000,0.0,0.0,0.0,0.0,0.0 +0,1.206100,0.0,0.0,0.0,0.0,0.0 +0,1.206200,0.0,0.0,0.0,0.0,0.0 +0,1.206300,0.0,0.0,0.0,0.0,0.0 +0,1.206400,0.0,0.0,0.0,0.0,0.0 +0,1.206500,0.0,0.0,0.0,0.0,0.0 +0,1.206600,0.0,0.0,0.0,0.0,0.0 +0,1.206700,0.0,0.0,0.0,0.0,0.0 +0,1.206800,0.0,0.0,0.0,0.0,0.0 +0,1.206900,0.0,0.0,0.0,0.0,0.0 +0,1.207000,0.0,0.0,0.0,0.0,0.0 +0,1.207100,0.0,0.0,0.0,0.0,0.0 +0,1.207200,0.0,0.0,0.0,0.0,0.0 +0,1.207300,0.0,0.0,0.0,0.0,0.0 +0,1.207400,0.0,0.0,0.0,0.0,0.0 +0,1.207500,0.0,0.0,0.0,0.0,0.0 +0,1.207600,0.0,0.0,0.0,0.0,0.0 +0,1.207700,0.0,0.0,0.0,0.0,0.0 +0,1.207800,0.0,0.0,0.0,0.0,0.0 +0,1.207900,0.0,0.0,0.0,0.0,0.0 +0,1.208000,0.0,0.0,0.0,0.0,0.0 +0,1.208100,0.0,0.0,0.0,0.0,0.0 +0,1.208200,0.0,0.0,0.0,0.0,0.0 +0,1.208300,0.0,0.0,0.0,0.0,0.0 +0,1.208400,0.0,0.0,0.0,0.0,0.0 +0,1.208500,0.0,0.0,0.0,0.0,0.0 +0,1.208600,0.0,0.0,0.0,0.0,0.0 +0,1.208700,0.0,0.0,0.0,0.0,0.0 +0,1.208800,0.0,0.0,0.0,0.0,0.0 +0,1.208900,0.0,0.0,0.0,0.0,0.0 +0,1.209000,0.0,0.0,0.0,0.0,0.0 +0,1.209100,0.0,0.0,0.0,0.0,0.0 +0,1.209200,0.0,0.0,0.0,0.0,0.0 +0,1.209300,0.0,0.0,0.0,0.0,0.0 +0,1.209400,0.0,0.0,0.0,0.0,0.0 +0,1.209500,0.0,0.0,0.0,0.0,0.0 +0,1.209600,0.0,0.0,0.0,0.0,0.0 +0,1.209700,0.0,0.0,0.0,0.0,0.0 +0,1.209800,0.0,0.0,0.0,0.0,0.0 +0,1.209900,0.0,0.0,0.0,0.0,0.0 +0,1.210000,0.0,0.0,0.0,0.0,0.0 +0,1.210100,0.0,0.0,0.0,0.0,0.0 +0,1.210200,0.0,0.0,0.0,0.0,0.0 +0,1.210300,0.0,0.0,0.0,0.0,0.0 +0,1.210400,0.0,0.0,0.0,0.0,0.0 +0,1.210500,0.0,0.0,0.0,0.0,0.0 +0,1.210600,0.0,0.0,0.0,0.0,0.0 +0,1.210700,0.0,0.0,0.0,0.0,0.0 +0,1.210800,0.0,0.0,0.0,0.0,0.0 +0,1.210900,0.0,0.0,0.0,0.0,0.0 +0,1.211000,0.0,0.0,0.0,0.0,0.0 +0,1.211100,0.0,0.0,0.0,0.0,0.0 +0,1.211200,0.0,0.0,0.0,0.0,0.0 +0,1.211300,0.0,0.0,0.0,0.0,0.0 +0,1.211400,0.0,0.0,0.0,0.0,0.0 +0,1.211500,0.0,0.0,0.0,0.0,0.0 +0,1.211600,0.0,0.0,0.0,0.0,0.0 +0,1.211700,0.0,0.0,0.0,0.0,0.0 +0,1.211800,0.0,0.0,0.0,0.0,0.0 +0,1.211900,0.0,0.0,0.0,0.0,0.0 +0,1.212000,0.0,0.0,0.0,0.0,0.0 +0,1.212100,0.0,0.0,0.0,0.0,0.0 +0,1.212200,0.0,0.0,0.0,0.0,0.0 +0,1.212300,0.0,0.0,0.0,0.0,0.0 +0,1.212400,0.0,0.0,0.0,0.0,0.0 +0,1.212500,0.0,0.0,0.0,0.0,0.0 +0,1.212600,0.0,0.0,0.0,0.0,0.0 +0,1.212700,0.0,0.0,0.0,0.0,0.0 +0,1.212800,0.0,0.0,0.0,0.0,0.0 +0,1.212900,0.0,0.0,0.0,0.0,0.0 +0,1.213000,0.0,0.0,0.0,0.0,0.0 +0,1.213100,0.0,0.0,0.0,0.0,0.0 +0,1.213200,0.0,0.0,0.0,0.0,0.0 +0,1.213300,0.0,0.0,0.0,0.0,0.0 +0,1.213400,0.0,0.0,0.0,0.0,0.0 +0,1.213500,0.0,0.0,0.0,0.0,0.0 +0,1.213600,0.0,0.0,0.0,0.0,0.0 +0,1.213700,0.0,0.0,0.0,0.0,0.0 +0,1.213800,0.0,0.0,0.0,0.0,0.0 +0,1.213900,0.0,0.0,0.0,0.0,0.0 +0,1.214000,0.0,0.0,0.0,0.0,0.0 +0,1.214100,0.0,0.0,0.0,0.0,0.0 +0,1.214200,0.0,0.0,0.0,0.0,0.0 +0,1.214300,0.0,0.0,0.0,0.0,0.0 +0,1.214400,0.0,0.0,0.0,0.0,0.0 +0,1.214500,0.0,0.0,0.0,0.0,0.0 +0,1.214600,0.0,0.0,0.0,0.0,0.0 +0,1.214700,0.0,0.0,0.0,0.0,0.0 +0,1.214800,0.0,0.0,0.0,0.0,0.0 +0,1.214900,0.0,0.0,0.0,0.0,0.0 +0,1.215000,0.0,0.0,0.0,0.0,0.0 +0,1.215100,0.0,0.0,0.0,0.0,0.0 +0,1.215200,0.0,0.0,0.0,0.0,0.0 +0,1.215300,0.0,0.0,0.0,0.0,0.0 +0,1.215400,0.0,0.0,0.0,0.0,0.0 +0,1.215500,0.0,0.0,0.0,0.0,0.0 +0,1.215600,0.0,0.0,0.0,0.0,0.0 +0,1.215700,0.0,0.0,0.0,0.0,0.0 +0,1.215800,0.0,0.0,0.0,0.0,0.0 +0,1.215900,0.0,0.0,0.0,0.0,0.0 +0,1.216000,0.0,0.0,0.0,0.0,0.0 +0,1.216100,0.0,0.0,0.0,0.0,0.0 +0,1.216200,0.0,0.0,0.0,0.0,0.0 +0,1.216300,0.0,0.0,0.0,0.0,0.0 +0,1.216400,0.0,0.0,0.0,0.0,0.0 +0,1.216500,0.0,0.0,0.0,0.0,0.0 +0,1.216600,0.0,0.0,0.0,0.0,0.0 +0,1.216700,0.0,0.0,0.0,0.0,0.0 +0,1.216800,0.0,0.0,0.0,0.0,0.0 +0,1.216900,0.0,0.0,0.0,0.0,0.0 +0,1.217000,0.0,0.0,0.0,0.0,0.0 +0,1.217100,0.0,0.0,0.0,0.0,0.0 +0,1.217200,0.0,0.0,0.0,0.0,0.0 +0,1.217300,0.0,0.0,0.0,0.0,0.0 +0,1.217400,0.0,0.0,0.0,0.0,0.0 +0,1.217500,0.0,0.0,0.0,0.0,0.0 +0,1.217600,0.0,0.0,0.0,0.0,0.0 +0,1.217700,0.0,0.0,0.0,0.0,0.0 +0,1.217800,0.0,0.0,0.0,0.0,0.0 +0,1.217900,0.0,0.0,0.0,0.0,0.0 +0,1.218000,0.0,0.0,0.0,0.0,0.0 +0,1.218100,0.0,0.0,0.0,0.0,0.0 +0,1.218200,0.0,0.0,0.0,0.0,0.0 +0,1.218300,0.0,0.0,0.0,0.0,0.0 +0,1.218400,0.0,0.0,0.0,0.0,0.0 +0,1.218500,0.0,0.0,0.0,0.0,0.0 +0,1.218600,0.0,0.0,0.0,0.0,0.0 +0,1.218700,0.0,0.0,0.0,0.0,0.0 +0,1.218800,0.0,0.0,0.0,0.0,0.0 +0,1.218900,0.0,0.0,0.0,0.0,0.0 +0,1.219000,0.0,0.0,0.0,0.0,0.0 +0,1.219100,0.0,0.0,0.0,0.0,0.0 +0,1.219200,0.0,0.0,0.0,0.0,0.0 +0,1.219300,0.0,0.0,0.0,0.0,0.0 +0,1.219400,0.0,0.0,0.0,0.0,0.0 +0,1.219500,0.0,0.0,0.0,0.0,0.0 +0,1.219600,0.0,0.0,0.0,0.0,0.0 +0,1.219700,0.0,0.0,0.0,0.0,0.0 +0,1.219800,0.0,0.0,0.0,0.0,0.0 +0,1.219900,0.0,0.0,0.0,0.0,0.0 +0,1.220000,0.0,0.0,0.0,0.0,0.0 +0,1.220100,0.0,0.0,0.0,0.0,0.0 +1,756.882441,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.220200,0.0,0.0,0.0,0.0,0.0 +0,1.220300,0.0,0.0,0.0,0.0,0.0 +0,1.220400,0.0,0.0,0.0,0.0,0.0 +0,1.220500,0.0,0.0,0.0,0.0,0.0 +0,1.220600,0.0,0.0,0.0,0.0,0.0 +0,1.220700,0.0,0.0,0.0,0.0,0.0 +0,1.220800,0.0,0.0,0.0,0.0,0.0 +0,1.220900,0.0,0.0,0.0,0.0,0.0 +0,1.221000,0.0,0.0,0.0,0.0,0.0 +0,1.221100,0.0,0.0,0.0,0.0,0.0 +0,1.221200,0.0,0.0,0.0,0.0,0.0 +0,1.221300,0.0,0.0,0.0,0.0,0.0 +0,1.221400,0.0,0.0,0.0,0.0,0.0 +0,1.221500,0.0,0.0,0.0,0.0,0.0 +0,1.221600,0.0,0.0,0.0,0.0,0.0 +0,1.221700,0.0,0.0,0.0,0.0,0.0 +0,1.221800,0.0,0.0,0.0,0.0,0.0 +0,1.221900,0.0,0.0,0.0,0.0,0.0 +0,1.222000,0.0,0.0,0.0,0.0,0.0 +0,1.222100,0.0,0.0,0.0,0.0,0.0 +0,1.222200,0.0,0.0,0.0,0.0,0.0 +0,1.222300,0.0,0.0,0.0,0.0,0.0 +0,1.222400,0.0,0.0,0.0,0.0,0.0 +0,1.222500,0.0,0.0,0.0,0.0,0.0 +0,1.222600,0.0,0.0,0.0,0.0,0.0 +0,1.222700,0.0,0.0,0.0,0.0,0.0 +0,1.222800,0.0,0.0,0.0,0.0,0.0 +0,1.222900,0.0,0.0,0.0,0.0,0.0 +0,1.223000,0.0,0.0,0.0,0.0,0.0 +0,1.223100,0.0,0.0,0.0,0.0,0.0 +0,1.223200,0.0,0.0,0.0,0.0,0.0 +0,1.223300,0.0,0.0,0.0,0.0,0.0 +0,1.223400,0.0,0.0,0.0,0.0,0.0 +0,1.223500,0.0,0.0,0.0,0.0,0.0 +0,1.223600,0.0,0.0,0.0,0.0,0.0 +0,1.223700,0.0,0.0,0.0,0.0,0.0 +0,1.223800,0.0,0.0,0.0,0.0,0.0 +0,1.223900,0.0,0.0,0.0,0.0,0.0 +0,1.224000,0.0,0.0,0.0,0.0,0.0 +0,1.224100,0.0,0.0,0.0,0.0,0.0 +0,1.224200,0.0,0.0,0.0,0.0,0.0 +0,1.224300,0.0,0.0,0.0,0.0,0.0 +0,1.224400,0.0,0.0,0.0,0.0,0.0 +0,1.224500,0.0,0.0,0.0,0.0,0.0 +0,1.224600,0.0,0.0,0.0,0.0,0.0 +0,1.224700,0.0,0.0,0.0,0.0,0.0 +0,1.224800,0.0,0.0,0.0,0.0,0.0 +0,1.224900,0.0,0.0,0.0,0.0,0.0 +0,1.225000,0.0,0.0,0.0,0.0,0.0 +0,1.225100,0.0,0.0,0.0,0.0,0.0 +0,1.225200,0.0,0.0,0.0,0.0,0.0 +0,1.225300,0.0,0.0,0.0,0.0,0.0 +0,1.225400,0.0,0.0,0.0,0.0,0.0 +0,1.225500,0.0,0.0,0.0,0.0,0.0 +0,1.225600,0.0,0.0,0.0,0.0,0.0 +0,1.225700,0.0,0.0,0.0,0.0,0.0 +0,1.225800,0.0,0.0,0.0,0.0,0.0 +0,1.225900,0.0,0.0,0.0,0.0,0.0 +0,1.226000,0.0,0.0,0.0,0.0,0.0 +0,1.226100,0.0,0.0,0.0,0.0,0.0 +0,1.226200,0.0,0.0,0.0,0.0,0.0 +0,1.226300,0.0,0.0,0.0,0.0,0.0 +0,1.226400,0.0,0.0,0.0,0.0,0.0 +0,1.226500,0.0,0.0,0.0,0.0,0.0 +0,1.226600,0.0,0.0,0.0,0.0,0.0 +0,1.226700,0.0,0.0,0.0,0.0,0.0 +0,1.226800,0.0,0.0,0.0,0.0,0.0 +0,1.226900,0.0,0.0,0.0,0.0,0.0 +0,1.227000,0.0,0.0,0.0,0.0,0.0 +0,1.227100,0.0,0.0,0.0,0.0,0.0 +0,1.227200,0.0,0.0,0.0,0.0,0.0 +0,1.227300,0.0,0.0,0.0,0.0,0.0 +0,1.227400,0.0,0.0,0.0,0.0,0.0 +0,1.227500,0.0,0.0,0.0,0.0,0.0 +0,1.227600,0.0,0.0,0.0,0.0,0.0 +0,1.227700,0.0,0.0,0.0,0.0,0.0 +0,1.227800,0.0,0.0,0.0,0.0,0.0 +0,1.227900,0.0,0.0,0.0,0.0,0.0 +0,1.228000,0.0,0.0,0.0,0.0,0.0 +0,1.228100,0.0,0.0,0.0,0.0,0.0 +0,1.228200,0.0,0.0,0.0,0.0,0.0 +0,1.228300,0.0,0.0,0.0,0.0,0.0 +0,1.228400,0.0,0.0,0.0,0.0,0.0 +0,1.228500,0.0,0.0,0.0,0.0,0.0 +0,1.228600,0.0,0.0,0.0,0.0,0.0 +0,1.228700,0.0,0.0,0.0,0.0,0.0 +0,1.228800,0.0,0.0,0.0,0.0,0.0 +0,1.228900,0.0,0.0,0.0,0.0,0.0 +0,1.229000,0.0,0.0,0.0,0.0,0.0 +0,1.229100,0.0,0.0,0.0,0.0,0.0 +0,1.229200,0.0,0.0,0.0,0.0,0.0 +0,1.229300,0.0,0.0,0.0,0.0,0.0 +0,1.229400,0.0,0.0,0.0,0.0,0.0 +0,1.229500,0.0,0.0,0.0,0.0,0.0 +0,1.229600,0.0,0.0,0.0,0.0,0.0 +0,1.229700,0.0,0.0,0.0,0.0,0.0 +0,1.229800,0.0,0.0,0.0,0.0,0.0 +0,1.229900,0.0,0.0,0.0,0.0,0.0 +0,1.230000,0.0,0.0,0.0,0.0,0.0 +0,1.230100,0.0,0.0,0.0,0.0,0.0 +0,1.230200,0.0,0.0,0.0,0.0,0.0 +0,1.230300,0.0,0.0,0.0,0.0,0.0 +0,1.230400,0.0,0.0,0.0,0.0,0.0 +0,1.230500,0.0,0.0,0.0,0.0,0.0 +0,1.230600,0.0,0.0,0.0,0.0,0.0 +0,1.230700,0.0,0.0,0.0,0.0,0.0 +0,1.230800,0.0,0.0,0.0,0.0,0.0 +0,1.230900,0.0,0.0,0.0,0.0,0.0 +0,1.231000,0.0,0.0,0.0,0.0,0.0 +0,1.231100,0.0,0.0,0.0,0.0,0.0 +0,1.231200,0.0,0.0,0.0,0.0,0.0 +0,1.231300,0.0,0.0,0.0,0.0,0.0 +0,1.231400,0.0,0.0,0.0,0.0,0.0 +0,1.231500,0.0,0.0,0.0,0.0,0.0 +0,1.231600,0.0,0.0,0.0,0.0,0.0 +0,1.231700,0.0,0.0,0.0,0.0,0.0 +0,1.231800,0.0,0.0,0.0,0.0,0.0 +0,1.231900,0.0,0.0,0.0,0.0,0.0 +0,1.232000,0.0,0.0,0.0,0.0,0.0 +0,1.232100,0.0,0.0,0.0,0.0,0.0 +0,1.232200,0.0,0.0,0.0,0.0,0.0 +0,1.232300,0.0,0.0,0.0,0.0,0.0 +0,1.232400,0.0,0.0,0.0,0.0,0.0 +0,1.232500,0.0,0.0,0.0,0.0,0.0 +0,1.232600,0.0,0.0,0.0,0.0,0.0 +0,1.232700,0.0,0.0,0.0,0.0,0.0 +0,1.232800,0.0,0.0,0.0,0.0,0.0 +0,1.232900,0.0,0.0,0.0,0.0,0.0 +0,1.233000,0.0,0.0,0.0,0.0,0.0 +0,1.233100,0.0,0.0,0.0,0.0,0.0 +0,1.233200,0.0,0.0,0.0,0.0,0.0 +0,1.233300,0.0,0.0,0.0,0.0,0.0 +0,1.233400,0.0,0.0,0.0,0.0,0.0 +0,1.233500,0.0,0.0,0.0,0.0,0.0 +0,1.233600,0.0,0.0,0.0,0.0,0.0 +0,1.233700,0.0,0.0,0.0,0.0,0.0 +0,1.233800,0.0,0.0,0.0,0.0,0.0 +0,1.233900,0.0,0.0,0.0,0.0,0.0 +0,1.234000,0.0,0.0,0.0,0.0,0.0 +0,1.234100,0.0,0.0,0.0,0.0,0.0 +0,1.234200,0.0,0.0,0.0,0.0,0.0 +0,1.234300,0.0,0.0,0.0,0.0,0.0 +0,1.234400,0.0,0.0,0.0,0.0,0.0 +0,1.234500,0.0,0.0,0.0,0.0,0.0 +0,1.234600,0.0,0.0,0.0,0.0,0.0 +0,1.234700,0.0,0.0,0.0,0.0,0.0 +0,1.234800,0.0,0.0,0.0,0.0,0.0 +0,1.234900,0.0,0.0,0.0,0.0,0.0 +0,1.235000,0.0,0.0,0.0,0.0,0.0 +0,1.235100,0.0,0.0,0.0,0.0,0.0 +0,1.235200,0.0,0.0,0.0,0.0,0.0 +0,1.235300,0.0,0.0,0.0,0.0,0.0 +0,1.235400,0.0,0.0,0.0,0.0,0.0 +0,1.235500,0.0,0.0,0.0,0.0,0.0 +0,1.235600,0.0,0.0,0.0,0.0,0.0 +0,1.235700,0.0,0.0,0.0,0.0,0.0 +0,1.235800,0.0,0.0,0.0,0.0,0.0 +0,1.235900,0.0,0.0,0.0,0.0,0.0 +0,1.236000,0.0,0.0,0.0,0.0,0.0 +0,1.236100,0.0,0.0,0.0,0.0,0.0 +0,1.236200,0.0,0.0,0.0,0.0,0.0 +0,1.236300,0.0,0.0,0.0,0.0,0.0 +0,1.236400,0.0,0.0,0.0,0.0,0.0 +0,1.236500,0.0,0.0,0.0,0.0,0.0 +0,1.236600,0.0,0.0,0.0,0.0,0.0 +0,1.236700,0.0,0.0,0.0,0.0,0.0 +0,1.236800,0.0,0.0,0.0,0.0,0.0 +0,1.236900,0.0,0.0,0.0,0.0,0.0 +0,1.237000,0.0,0.0,0.0,0.0,0.0 +0,1.237100,0.0,0.0,0.0,0.0,0.0 +0,1.237200,0.0,0.0,0.0,0.0,0.0 +0,1.237300,0.0,0.0,0.0,0.0,0.0 +0,1.237400,0.0,0.0,0.0,0.0,0.0 +0,1.237500,0.0,0.0,0.0,0.0,0.0 +0,1.237600,0.0,0.0,0.0,0.0,0.0 +0,1.237700,0.0,0.0,0.0,0.0,0.0 +0,1.237800,0.0,0.0,0.0,0.0,0.0 +0,1.237900,0.0,0.0,0.0,0.0,0.0 +0,1.238000,0.0,0.0,0.0,0.0,0.0 +0,1.238100,0.0,0.0,0.0,0.0,0.0 +0,1.238200,0.0,0.0,0.0,0.0,0.0 +0,1.238300,0.0,0.0,0.0,0.0,0.0 +0,1.238400,0.0,0.0,0.0,0.0,0.0 +0,1.238500,0.0,0.0,0.0,0.0,0.0 +0,1.238600,0.0,0.0,0.0,0.0,0.0 +0,1.238700,0.0,0.0,0.0,0.0,0.0 +0,1.238800,0.0,0.0,0.0,0.0,0.0 +0,1.238900,0.0,0.0,0.0,0.0,0.0 +0,1.239000,0.0,0.0,0.0,0.0,0.0 +0,1.239100,0.0,0.0,0.0,0.0,0.0 +0,1.239200,0.0,0.0,0.0,0.0,0.0 +0,1.239300,0.0,0.0,0.0,0.0,0.0 +0,1.239400,0.0,0.0,0.0,0.0,0.0 +0,1.239500,0.0,0.0,0.0,0.0,0.0 +0,1.239600,0.0,0.0,0.0,0.0,0.0 +0,1.239700,0.0,0.0,0.0,0.0,0.0 +0,1.239800,0.0,0.0,0.0,0.0,0.0 +0,1.239900,0.0,0.0,0.0,0.0,0.0 +0,1.240000,0.0,0.0,0.0,0.0,0.0 +0,1.240100,0.0,0.0,0.0,0.0,0.0 +1,794.715000,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.240200,0.0,0.0,0.0,0.0,0.0 +0,1.240300,0.0,0.0,0.0,0.0,0.0 +0,1.240400,0.0,0.0,0.0,0.0,0.0 +0,1.240500,0.0,0.0,0.0,0.0,0.0 +0,1.240600,0.0,0.0,0.0,0.0,0.0 +0,1.240700,0.0,0.0,0.0,0.0,0.0 +0,1.240800,0.0,0.0,0.0,0.0,0.0 +0,1.240900,0.0,0.0,0.0,0.0,0.0 +0,1.241000,0.0,0.0,0.0,0.0,0.0 +0,1.241100,0.0,0.0,0.0,0.0,0.0 +0,1.241200,0.0,0.0,0.0,0.0,0.0 +0,1.241300,0.0,0.0,0.0,0.0,0.0 +0,1.241400,0.0,0.0,0.0,0.0,0.0 +0,1.241500,0.0,0.0,0.0,0.0,0.0 +0,1.241600,0.0,0.0,0.0,0.0,0.0 +0,1.241700,0.0,0.0,0.0,0.0,0.0 +0,1.241800,0.0,0.0,0.0,0.0,0.0 +0,1.241900,0.0,0.0,0.0,0.0,0.0 +0,1.242000,0.0,0.0,0.0,0.0,0.0 +0,1.242100,0.0,0.0,0.0,0.0,0.0 +0,1.242200,0.0,0.0,0.0,0.0,0.0 +0,1.242300,0.0,0.0,0.0,0.0,0.0 +0,1.242400,0.0,0.0,0.0,0.0,0.0 +0,1.242500,0.0,0.0,0.0,0.0,0.0 +0,1.242600,0.0,0.0,0.0,0.0,0.0 +0,1.242700,0.0,0.0,0.0,0.0,0.0 +0,1.242800,0.0,0.0,0.0,0.0,0.0 +0,1.242900,0.0,0.0,0.0,0.0,0.0 +0,1.243000,0.0,0.0,0.0,0.0,0.0 +0,1.243100,0.0,0.0,0.0,0.0,0.0 +0,1.243200,0.0,0.0,0.0,0.0,0.0 +0,1.243300,0.0,0.0,0.0,0.0,0.0 +0,1.243400,0.0,0.0,0.0,0.0,0.0 +0,1.243500,0.0,0.0,0.0,0.0,0.0 +0,1.243600,0.0,0.0,0.0,0.0,0.0 +0,1.243700,0.0,0.0,0.0,0.0,0.0 +0,1.243800,0.0,0.0,0.0,0.0,0.0 +0,1.243900,0.0,0.0,0.0,0.0,0.0 +0,1.244000,0.0,0.0,0.0,0.0,0.0 +0,1.244100,0.0,0.0,0.0,0.0,0.0 +0,1.244200,0.0,0.0,0.0,0.0,0.0 +0,1.244300,0.0,0.0,0.0,0.0,0.0 +0,1.244400,0.0,0.0,0.0,0.0,0.0 +0,1.244500,0.0,0.0,0.0,0.0,0.0 +0,1.244600,0.0,0.0,0.0,0.0,0.0 +0,1.244700,0.0,0.0,0.0,0.0,0.0 +0,1.244800,0.0,0.0,0.0,0.0,0.0 +0,1.244900,0.0,0.0,0.0,0.0,0.0 +0,1.245000,0.0,0.0,0.0,0.0,0.0 +0,1.245100,0.0,0.0,0.0,0.0,0.0 +0,1.245200,0.0,0.0,0.0,0.0,0.0 +0,1.245300,0.0,0.0,0.0,0.0,0.0 +0,1.245400,0.0,0.0,0.0,0.0,0.0 +0,1.245500,0.0,0.0,0.0,0.0,0.0 +0,1.245600,0.0,0.0,0.0,0.0,0.0 +0,1.245700,0.0,0.0,0.0,0.0,0.0 +0,1.245800,0.0,0.0,0.0,0.0,0.0 +0,1.245900,0.0,0.0,0.0,0.0,0.0 +0,1.246000,0.0,0.0,0.0,0.0,0.0 +0,1.246100,0.0,0.0,0.0,0.0,0.0 +0,1.246200,0.0,0.0,0.0,0.0,0.0 +0,1.246300,0.0,0.0,0.0,0.0,0.0 +0,1.246400,0.0,0.0,0.0,0.0,0.0 +0,1.246500,0.0,0.0,0.0,0.0,0.0 +0,1.246600,0.0,0.0,0.0,0.0,0.0 +0,1.246700,0.0,0.0,0.0,0.0,0.0 +0,1.246800,0.0,0.0,0.0,0.0,0.0 +0,1.246900,0.0,0.0,0.0,0.0,0.0 +0,1.247000,0.0,0.0,0.0,0.0,0.0 +0,1.247100,0.0,0.0,0.0,0.0,0.0 +0,1.247200,0.0,0.0,0.0,0.0,0.0 +0,1.247300,0.0,0.0,0.0,0.0,0.0 +0,1.247400,0.0,0.0,0.0,0.0,0.0 +0,1.247500,0.0,0.0,0.0,0.0,0.0 +0,1.247600,0.0,0.0,0.0,0.0,0.0 +0,1.247700,0.0,0.0,0.0,0.0,0.0 +0,1.247800,0.0,0.0,0.0,0.0,0.0 +0,1.247900,0.0,0.0,0.0,0.0,0.0 +0,1.248000,0.0,0.0,0.0,0.0,0.0 +0,1.248100,0.0,0.0,0.0,0.0,0.0 +0,1.248200,0.0,0.0,0.0,0.0,0.0 +0,1.248300,0.0,0.0,0.0,0.0,0.0 +0,1.248400,0.0,0.0,0.0,0.0,0.0 +0,1.248500,0.0,0.0,0.0,0.0,0.0 +0,1.248600,0.0,0.0,0.0,0.0,0.0 +0,1.248700,0.0,0.0,0.0,0.0,0.0 +0,1.248800,0.0,0.0,0.0,0.0,0.0 +0,1.248900,0.0,0.0,0.0,0.0,0.0 +0,1.249000,0.0,0.0,0.0,0.0,0.0 +0,1.249100,0.0,0.0,0.0,0.0,0.0 +0,1.249200,0.0,0.0,0.0,0.0,0.0 +0,1.249300,0.0,0.0,0.0,0.0,0.0 +0,1.249400,0.0,0.0,0.0,0.0,0.0 +0,1.249500,0.0,0.0,0.0,0.0,0.0 +0,1.249600,0.0,0.0,0.0,0.0,0.0 +0,1.249700,0.0,0.0,0.0,0.0,0.0 +0,1.249800,0.0,0.0,0.0,0.0,0.0 +0,1.249900,0.0,0.0,0.0,0.0,0.0 +0,1.250000,0.0,0.0,0.0,0.0,0.0 +0,1.250100,0.0,0.0,0.0,0.0,0.0 +0,1.250200,0.0,0.0,0.0,0.0,0.0 +0,1.250300,0.0,0.0,0.0,0.0,0.0 +0,1.250400,0.0,0.0,0.0,0.0,0.0 +0,1.250500,0.0,0.0,0.0,0.0,0.0 +0,1.250600,0.0,0.0,0.0,0.0,0.0 +0,1.250700,0.0,0.0,0.0,0.0,0.0 +0,1.250800,0.0,0.0,0.0,0.0,0.0 +0,1.250900,0.0,0.0,0.0,0.0,0.0 +0,1.251000,0.0,0.0,0.0,0.0,0.0 +0,1.251100,0.0,0.0,0.0,0.0,0.0 +0,1.251200,0.0,0.0,0.0,0.0,0.0 +0,1.251300,0.0,0.0,0.0,0.0,0.0 +0,1.251400,0.0,0.0,0.0,0.0,0.0 +0,1.251500,0.0,0.0,0.0,0.0,0.0 +0,1.251600,0.0,0.0,0.0,0.0,0.0 +0,1.251700,0.0,0.0,0.0,0.0,0.0 +0,1.251800,0.0,0.0,0.0,0.0,0.0 +0,1.251900,0.0,0.0,0.0,0.0,0.0 +0,1.252000,0.0,0.0,0.0,0.0,0.0 +0,1.252100,0.0,0.0,0.0,0.0,0.0 +0,1.252200,0.0,0.0,0.0,0.0,0.0 +0,1.252300,0.0,0.0,0.0,0.0,0.0 +0,1.252400,0.0,0.0,0.0,0.0,0.0 +0,1.252500,0.0,0.0,0.0,0.0,0.0 +0,1.252600,0.0,0.0,0.0,0.0,0.0 +0,1.252700,0.0,0.0,0.0,0.0,0.0 +0,1.252800,0.0,0.0,0.0,0.0,0.0 +0,1.252900,0.0,0.0,0.0,0.0,0.0 +0,1.253000,0.0,0.0,0.0,0.0,0.0 +0,1.253100,0.0,0.0,0.0,0.0,0.0 +0,1.253200,0.0,0.0,0.0,0.0,0.0 +0,1.253300,0.0,0.0,0.0,0.0,0.0 +0,1.253400,0.0,0.0,0.0,0.0,0.0 +0,1.253500,0.0,0.0,0.0,0.0,0.0 +0,1.253600,0.0,0.0,0.0,0.0,0.0 +0,1.253700,0.0,0.0,0.0,0.0,0.0 +0,1.253800,0.0,0.0,0.0,0.0,0.0 +0,1.253900,0.0,0.0,0.0,0.0,0.0 +0,1.254000,0.0,0.0,0.0,0.0,0.0 +0,1.254100,0.0,0.0,0.0,0.0,0.0 +0,1.254200,0.0,0.0,0.0,0.0,0.0 +0,1.254300,0.0,0.0,0.0,0.0,0.0 +0,1.254400,0.0,0.0,0.0,0.0,0.0 +0,1.254500,0.0,0.0,0.0,0.0,0.0 +0,1.254600,0.0,0.0,0.0,0.0,0.0 +0,1.254700,0.0,0.0,0.0,0.0,0.0 +0,1.254800,0.0,0.0,0.0,0.0,0.0 +0,1.254900,0.0,0.0,0.0,0.0,0.0 +0,1.255000,0.0,0.0,0.0,0.0,0.0 +0,1.255100,0.0,0.0,0.0,0.0,0.0 +0,1.255200,0.0,0.0,0.0,0.0,0.0 +0,1.255300,0.0,0.0,0.0,0.0,0.0 +0,1.255400,0.0,0.0,0.0,0.0,0.0 +0,1.255500,0.0,0.0,0.0,0.0,0.0 +0,1.255600,0.0,0.0,0.0,0.0,0.0 +0,1.255700,0.0,0.0,0.0,0.0,0.0 +0,1.255800,0.0,0.0,0.0,0.0,0.0 +0,1.255900,0.0,0.0,0.0,0.0,0.0 +0,1.256000,0.0,0.0,0.0,0.0,0.0 +0,1.256100,0.0,0.0,0.0,0.0,0.0 +0,1.256200,0.0,0.0,0.0,0.0,0.0 +0,1.256300,0.0,0.0,0.0,0.0,0.0 +0,1.256400,0.0,0.0,0.0,0.0,0.0 +0,1.256500,0.0,0.0,0.0,0.0,0.0 +0,1.256600,0.0,0.0,0.0,0.0,0.0 +0,1.256700,0.0,0.0,0.0,0.0,0.0 +0,1.256800,0.0,0.0,0.0,0.0,0.0 +0,1.256900,0.0,0.0,0.0,0.0,0.0 +0,1.257000,0.0,0.0,0.0,0.0,0.0 +0,1.257100,0.0,0.0,0.0,0.0,0.0 +0,1.257200,0.0,0.0,0.0,0.0,0.0 +0,1.257300,0.0,0.0,0.0,0.0,0.0 +0,1.257400,0.0,0.0,0.0,0.0,0.0 +0,1.257500,0.0,0.0,0.0,0.0,0.0 +0,1.257600,0.0,0.0,0.0,0.0,0.0 +0,1.257700,0.0,0.0,0.0,0.0,0.0 +0,1.257800,0.0,0.0,0.0,0.0,0.0 +0,1.257900,0.0,0.0,0.0,0.0,0.0 +0,1.258000,0.0,0.0,0.0,0.0,0.0 +0,1.258100,0.0,0.0,0.0,0.0,0.0 +0,1.258200,0.0,0.0,0.0,0.0,0.0 +0,1.258300,0.0,0.0,0.0,0.0,0.0 +0,1.258400,0.0,0.0,0.0,0.0,0.0 +0,1.258500,0.0,0.0,0.0,0.0,0.0 +0,1.258600,0.0,0.0,0.0,0.0,0.0 +0,1.258700,0.0,0.0,0.0,0.0,0.0 +0,1.258800,0.0,0.0,0.0,0.0,0.0 +0,1.258900,0.0,0.0,0.0,0.0,0.0 +0,1.259000,0.0,0.0,0.0,0.0,0.0 +0,1.259100,0.0,0.0,0.0,0.0,0.0 +0,1.259200,0.0,0.0,0.0,0.0,0.0 +0,1.259300,0.0,0.0,0.0,0.0,0.0 +0,1.259400,0.0,0.0,0.0,0.0,0.0 +0,1.259500,0.0,0.0,0.0,0.0,0.0 +0,1.259600,0.0,0.0,0.0,0.0,0.0 +0,1.259700,0.0,0.0,0.0,0.0,0.0 +0,1.259800,0.0,0.0,0.0,0.0,0.0 +0,1.259900,0.0,0.0,0.0,0.0,0.0 +0,1.260000,0.0,0.0,0.0,0.0,0.0 +0,1.260100,0.0,0.0,0.0,0.0,0.0 +1,833.787709,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.260200,0.0,0.0,0.0,0.0,0.0 +0,1.260300,0.0,0.0,0.0,0.0,0.0 +0,1.260400,0.0,0.0,0.0,0.0,0.0 +0,1.260500,0.0,0.0,0.0,0.0,0.0 +0,1.260600,0.0,0.0,0.0,0.0,0.0 +0,1.260700,0.0,0.0,0.0,0.0,0.0 +0,1.260800,0.0,0.0,0.0,0.0,0.0 +0,1.260900,0.0,0.0,0.0,0.0,0.0 +0,1.261000,0.0,0.0,0.0,0.0,0.0 +0,1.261100,0.0,0.0,0.0,0.0,0.0 +0,1.261200,0.0,0.0,0.0,0.0,0.0 +0,1.261300,0.0,0.0,0.0,0.0,0.0 +0,1.261400,0.0,0.0,0.0,0.0,0.0 +0,1.261500,0.0,0.0,0.0,0.0,0.0 +0,1.261600,0.0,0.0,0.0,0.0,0.0 +0,1.261700,0.0,0.0,0.0,0.0,0.0 +0,1.261800,0.0,0.0,0.0,0.0,0.0 +0,1.261900,0.0,0.0,0.0,0.0,0.0 +0,1.262000,0.0,0.0,0.0,0.0,0.0 +0,1.262100,0.0,0.0,0.0,0.0,0.0 +0,1.262200,0.0,0.0,0.0,0.0,0.0 +0,1.262300,0.0,0.0,0.0,0.0,0.0 +0,1.262400,0.0,0.0,0.0,0.0,0.0 +0,1.262500,0.0,0.0,0.0,0.0,0.0 +0,1.262600,0.0,0.0,0.0,0.0,0.0 +0,1.262700,0.0,0.0,0.0,0.0,0.0 +0,1.262800,0.0,0.0,0.0,0.0,0.0 +0,1.262900,0.0,0.0,0.0,0.0,0.0 +0,1.263000,0.0,0.0,0.0,0.0,0.0 +0,1.263100,0.0,0.0,0.0,0.0,0.0 +0,1.263200,0.0,0.0,0.0,0.0,0.0 +0,1.263300,0.0,0.0,0.0,0.0,0.0 +0,1.263400,0.0,0.0,0.0,0.0,0.0 +0,1.263500,0.0,0.0,0.0,0.0,0.0 +0,1.263600,0.0,0.0,0.0,0.0,0.0 +0,1.263700,0.0,0.0,0.0,0.0,0.0 +0,1.263800,0.0,0.0,0.0,0.0,0.0 +0,1.263900,0.0,0.0,0.0,0.0,0.0 +0,1.264000,0.0,0.0,0.0,0.0,0.0 +0,1.264100,0.0,0.0,0.0,0.0,0.0 +0,1.264200,0.0,0.0,0.0,0.0,0.0 +0,1.264300,0.0,0.0,0.0,0.0,0.0 +0,1.264400,0.0,0.0,0.0,0.0,0.0 +0,1.264500,0.0,0.0,0.0,0.0,0.0 +0,1.264600,0.0,0.0,0.0,0.0,0.0 +0,1.264700,0.0,0.0,0.0,0.0,0.0 +0,1.264800,0.0,0.0,0.0,0.0,0.0 +0,1.264900,0.0,0.0,0.0,0.0,0.0 +0,1.265000,0.0,0.0,0.0,0.0,0.0 +0,1.265100,0.0,0.0,0.0,0.0,0.0 +0,1.265200,0.0,0.0,0.0,0.0,0.0 +0,1.265300,0.0,0.0,0.0,0.0,0.0 +0,1.265400,0.0,0.0,0.0,0.0,0.0 +0,1.265500,0.0,0.0,0.0,0.0,0.0 +0,1.265600,0.0,0.0,0.0,0.0,0.0 +0,1.265700,0.0,0.0,0.0,0.0,0.0 +0,1.265800,0.0,0.0,0.0,0.0,0.0 +0,1.265900,0.0,0.0,0.0,0.0,0.0 +0,1.266000,0.0,0.0,0.0,0.0,0.0 +0,1.266100,0.0,0.0,0.0,0.0,0.0 +0,1.266200,0.0,0.0,0.0,0.0,0.0 +0,1.266300,0.0,0.0,0.0,0.0,0.0 +0,1.266400,0.0,0.0,0.0,0.0,0.0 +0,1.266500,0.0,0.0,0.0,0.0,0.0 +0,1.266600,0.0,0.0,0.0,0.0,0.0 +0,1.266700,0.0,0.0,0.0,0.0,0.0 +0,1.266800,0.0,0.0,0.0,0.0,0.0 +0,1.266900,0.0,0.0,0.0,0.0,0.0 +0,1.267000,0.0,0.0,0.0,0.0,0.0 +0,1.267100,0.0,0.0,0.0,0.0,0.0 +0,1.267200,0.0,0.0,0.0,0.0,0.0 +0,1.267300,0.0,0.0,0.0,0.0,0.0 +0,1.267400,0.0,0.0,0.0,0.0,0.0 +0,1.267500,0.0,0.0,0.0,0.0,0.0 +0,1.267600,0.0,0.0,0.0,0.0,0.0 +0,1.267700,0.0,0.0,0.0,0.0,0.0 +0,1.267800,0.0,0.0,0.0,0.0,0.0 +0,1.267900,0.0,0.0,0.0,0.0,0.0 +0,1.268000,0.0,0.0,0.0,0.0,0.0 +0,1.268100,0.0,0.0,0.0,0.0,0.0 +0,1.268200,0.0,0.0,0.0,0.0,0.0 +0,1.268300,0.0,0.0,0.0,0.0,0.0 +0,1.268400,0.0,0.0,0.0,0.0,0.0 +0,1.268500,0.0,0.0,0.0,0.0,0.0 +0,1.268600,0.0,0.0,0.0,0.0,0.0 +0,1.268700,0.0,0.0,0.0,0.0,0.0 +0,1.268800,0.0,0.0,0.0,0.0,0.0 +0,1.268900,0.0,0.0,0.0,0.0,0.0 +0,1.269000,0.0,0.0,0.0,0.0,0.0 +0,1.269100,0.0,0.0,0.0,0.0,0.0 +0,1.269200,0.0,0.0,0.0,0.0,0.0 +0,1.269300,0.0,0.0,0.0,0.0,0.0 +0,1.269400,0.0,0.0,0.0,0.0,0.0 +0,1.269500,0.0,0.0,0.0,0.0,0.0 +0,1.269600,0.0,0.0,0.0,0.0,0.0 +0,1.269700,0.0,0.0,0.0,0.0,0.0 +0,1.269800,0.0,0.0,0.0,0.0,0.0 +0,1.269900,0.0,0.0,0.0,0.0,0.0 +0,1.270000,0.0,0.0,0.0,0.0,0.0 +0,1.270100,0.0,0.0,0.0,0.0,0.0 +0,1.270200,0.0,0.0,0.0,0.0,0.0 +0,1.270300,0.0,0.0,0.0,0.0,0.0 +0,1.270400,0.0,0.0,0.0,0.0,0.0 +0,1.270500,0.0,0.0,0.0,0.0,0.0 +0,1.270600,0.0,0.0,0.0,0.0,0.0 +0,1.270700,0.0,0.0,0.0,0.0,0.0 +0,1.270800,0.0,0.0,0.0,0.0,0.0 +0,1.270900,0.0,0.0,0.0,0.0,0.0 +0,1.271000,0.0,0.0,0.0,0.0,0.0 +0,1.271100,0.0,0.0,0.0,0.0,0.0 +0,1.271200,0.0,0.0,0.0,0.0,0.0 +0,1.271300,0.0,0.0,0.0,0.0,0.0 +0,1.271400,0.0,0.0,0.0,0.0,0.0 +0,1.271500,0.0,0.0,0.0,0.0,0.0 +0,1.271600,0.0,0.0,0.0,0.0,0.0 +0,1.271700,0.0,0.0,0.0,0.0,0.0 +0,1.271800,0.0,0.0,0.0,0.0,0.0 +0,1.271900,0.0,0.0,0.0,0.0,0.0 +0,1.272000,0.0,0.0,0.0,0.0,0.0 +0,1.272100,0.0,0.0,0.0,0.0,0.0 +0,1.272200,0.0,0.0,0.0,0.0,0.0 +0,1.272300,0.0,0.0,0.0,0.0,0.0 +0,1.272400,0.0,0.0,0.0,0.0,0.0 +0,1.272500,0.0,0.0,0.0,0.0,0.0 +0,1.272600,0.0,0.0,0.0,0.0,0.0 +0,1.272700,0.0,0.0,0.0,0.0,0.0 +0,1.272800,0.0,0.0,0.0,0.0,0.0 +0,1.272900,0.0,0.0,0.0,0.0,0.0 +0,1.273000,0.0,0.0,0.0,0.0,0.0 +0,1.273100,0.0,0.0,0.0,0.0,0.0 +0,1.273200,0.0,0.0,0.0,0.0,0.0 +0,1.273300,0.0,0.0,0.0,0.0,0.0 +0,1.273400,0.0,0.0,0.0,0.0,0.0 +0,1.273500,0.0,0.0,0.0,0.0,0.0 +0,1.273600,0.0,0.0,0.0,0.0,0.0 +0,1.273700,0.0,0.0,0.0,0.0,0.0 +0,1.273800,0.0,0.0,0.0,0.0,0.0 +0,1.273900,0.0,0.0,0.0,0.0,0.0 +0,1.274000,0.0,0.0,0.0,0.0,0.0 +0,1.274100,0.0,0.0,0.0,0.0,0.0 +0,1.274200,0.0,0.0,0.0,0.0,0.0 +0,1.274300,0.0,0.0,0.0,0.0,0.0 +0,1.274400,0.0,0.0,0.0,0.0,0.0 +0,1.274500,0.0,0.0,0.0,0.0,0.0 +0,1.274600,0.0,0.0,0.0,0.0,0.0 +0,1.274700,0.0,0.0,0.0,0.0,0.0 +0,1.274800,0.0,0.0,0.0,0.0,0.0 +0,1.274900,0.0,0.0,0.0,0.0,0.0 +0,1.275000,0.0,0.0,0.0,0.0,0.0 +0,1.275100,0.0,0.0,0.0,0.0,0.0 +0,1.275200,0.0,0.0,0.0,0.0,0.0 +0,1.275300,0.0,0.0,0.0,0.0,0.0 +0,1.275400,0.0,0.0,0.0,0.0,0.0 +0,1.275500,0.0,0.0,0.0,0.0,0.0 +0,1.275600,0.0,0.0,0.0,0.0,0.0 +0,1.275700,0.0,0.0,0.0,0.0,0.0 +0,1.275800,0.0,0.0,0.0,0.0,0.0 +0,1.275900,0.0,0.0,0.0,0.0,0.0 +0,1.276000,0.0,0.0,0.0,0.0,0.0 +0,1.276100,0.0,0.0,0.0,0.0,0.0 +0,1.276200,0.0,0.0,0.0,0.0,0.0 +0,1.276300,0.0,0.0,0.0,0.0,0.0 +0,1.276400,0.0,0.0,0.0,0.0,0.0 +0,1.276500,0.0,0.0,0.0,0.0,0.0 +0,1.276600,0.0,0.0,0.0,0.0,0.0 +0,1.276700,0.0,0.0,0.0,0.0,0.0 +0,1.276800,0.0,0.0,0.0,0.0,0.0 +0,1.276900,0.0,0.0,0.0,0.0,0.0 +0,1.277000,0.0,0.0,0.0,0.0,0.0 +0,1.277100,0.0,0.0,0.0,0.0,0.0 +0,1.277200,0.0,0.0,0.0,0.0,0.0 +0,1.277300,0.0,0.0,0.0,0.0,0.0 +0,1.277400,0.0,0.0,0.0,0.0,0.0 +0,1.277500,0.0,0.0,0.0,0.0,0.0 +0,1.277600,0.0,0.0,0.0,0.0,0.0 +0,1.277700,0.0,0.0,0.0,0.0,0.0 +0,1.277800,0.0,0.0,0.0,0.0,0.0 +0,1.277900,0.0,0.0,0.0,0.0,0.0 +0,1.278000,0.0,0.0,0.0,0.0,0.0 +0,1.278100,0.0,0.0,0.0,0.0,0.0 +0,1.278200,0.0,0.0,0.0,0.0,0.0 +0,1.278300,0.0,0.0,0.0,0.0,0.0 +0,1.278400,0.0,0.0,0.0,0.0,0.0 +0,1.278500,0.0,0.0,0.0,0.0,0.0 +0,1.278600,0.0,0.0,0.0,0.0,0.0 +0,1.278700,0.0,0.0,0.0,0.0,0.0 +0,1.278800,0.0,0.0,0.0,0.0,0.0 +0,1.278900,0.0,0.0,0.0,0.0,0.0 +0,1.279000,0.0,0.0,0.0,0.0,0.0 +0,1.279100,0.0,0.0,0.0,0.0,0.0 +0,1.279200,0.0,0.0,0.0,0.0,0.0 +0,1.279300,0.0,0.0,0.0,0.0,0.0 +0,1.279400,0.0,0.0,0.0,0.0,0.0 +0,1.279500,0.0,0.0,0.0,0.0,0.0 +0,1.279600,0.0,0.0,0.0,0.0,0.0 +0,1.279700,0.0,0.0,0.0,0.0,0.0 +0,1.279800,0.0,0.0,0.0,0.0,0.0 +0,1.279900,0.0,0.0,0.0,0.0,0.0 +0,1.280000,0.0,0.0,0.0,0.0,0.0 +0,1.280100,0.0,0.0,0.0,0.0,0.0 +1,874.120568,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.280200,0.0,0.0,0.0,0.0,0.0 +0,1.280300,0.0,0.0,0.0,0.0,0.0 +0,1.280400,0.0,0.0,0.0,0.0,0.0 +0,1.280500,0.0,0.0,0.0,0.0,0.0 +0,1.280600,0.0,0.0,0.0,0.0,0.0 +0,1.280700,0.0,0.0,0.0,0.0,0.0 +0,1.280800,0.0,0.0,0.0,0.0,0.0 +0,1.280900,0.0,0.0,0.0,0.0,0.0 +0,1.281000,0.0,0.0,0.0,0.0,0.0 +0,1.281100,0.0,0.0,0.0,0.0,0.0 +0,1.281200,0.0,0.0,0.0,0.0,0.0 +0,1.281300,0.0,0.0,0.0,0.0,0.0 +0,1.281400,0.0,0.0,0.0,0.0,0.0 +0,1.281500,0.0,0.0,0.0,0.0,0.0 +0,1.281600,0.0,0.0,0.0,0.0,0.0 +0,1.281700,0.0,0.0,0.0,0.0,0.0 +0,1.281800,0.0,0.0,0.0,0.0,0.0 +0,1.281900,0.0,0.0,0.0,0.0,0.0 +0,1.282000,0.0,0.0,0.0,0.0,0.0 +0,1.282100,0.0,0.0,0.0,0.0,0.0 +0,1.282200,0.0,0.0,0.0,0.0,0.0 +0,1.282300,0.0,0.0,0.0,0.0,0.0 +0,1.282400,0.0,0.0,0.0,0.0,0.0 +0,1.282500,0.0,0.0,0.0,0.0,0.0 +0,1.282600,0.0,0.0,0.0,0.0,0.0 +0,1.282700,0.0,0.0,0.0,0.0,0.0 +0,1.282800,0.0,0.0,0.0,0.0,0.0 +0,1.282900,0.0,0.0,0.0,0.0,0.0 +0,1.283000,0.0,0.0,0.0,0.0,0.0 +0,1.283100,0.0,0.0,0.0,0.0,0.0 +0,1.283200,0.0,0.0,0.0,0.0,0.0 +0,1.283300,0.0,0.0,0.0,0.0,0.0 +0,1.283400,0.0,0.0,0.0,0.0,0.0 +0,1.283500,0.0,0.0,0.0,0.0,0.0 +0,1.283600,0.0,0.0,0.0,0.0,0.0 +0,1.283700,0.0,0.0,0.0,0.0,0.0 +0,1.283800,0.0,0.0,0.0,0.0,0.0 +0,1.283900,0.0,0.0,0.0,0.0,0.0 +0,1.284000,0.0,0.0,0.0,0.0,0.0 +0,1.284100,0.0,0.0,0.0,0.0,0.0 +0,1.284200,0.0,0.0,0.0,0.0,0.0 +0,1.284300,0.0,0.0,0.0,0.0,0.0 +0,1.284400,0.0,0.0,0.0,0.0,0.0 +0,1.284500,0.0,0.0,0.0,0.0,0.0 +0,1.284600,0.0,0.0,0.0,0.0,0.0 +0,1.284700,0.0,0.0,0.0,0.0,0.0 +0,1.284800,0.0,0.0,0.0,0.0,0.0 +0,1.284900,0.0,0.0,0.0,0.0,0.0 +0,1.285000,0.0,0.0,0.0,0.0,0.0 +0,1.285100,0.0,0.0,0.0,0.0,0.0 +0,1.285200,0.0,0.0,0.0,0.0,0.0 +0,1.285300,0.0,0.0,0.0,0.0,0.0 +0,1.285400,0.0,0.0,0.0,0.0,0.0 +0,1.285500,0.0,0.0,0.0,0.0,0.0 +0,1.285600,0.0,0.0,0.0,0.0,0.0 +0,1.285700,0.0,0.0,0.0,0.0,0.0 +0,1.285800,0.0,0.0,0.0,0.0,0.0 +0,1.285900,0.0,0.0,0.0,0.0,0.0 +0,1.286000,0.0,0.0,0.0,0.0,0.0 +0,1.286100,0.0,0.0,0.0,0.0,0.0 +0,1.286200,0.0,0.0,0.0,0.0,0.0 +0,1.286300,0.0,0.0,0.0,0.0,0.0 +0,1.286400,0.0,0.0,0.0,0.0,0.0 +0,1.286500,0.0,0.0,0.0,0.0,0.0 +0,1.286600,0.0,0.0,0.0,0.0,0.0 +0,1.286700,0.0,0.0,0.0,0.0,0.0 +0,1.286800,0.0,0.0,0.0,0.0,0.0 +0,1.286900,0.0,0.0,0.0,0.0,0.0 +0,1.287000,0.0,0.0,0.0,0.0,0.0 +0,1.287100,0.0,0.0,0.0,0.0,0.0 +0,1.287200,0.0,0.0,0.0,0.0,0.0 +0,1.287300,0.0,0.0,0.0,0.0,0.0 +0,1.287400,0.0,0.0,0.0,0.0,0.0 +0,1.287500,0.0,0.0,0.0,0.0,0.0 +0,1.287600,0.0,0.0,0.0,0.0,0.0 +0,1.287700,0.0,0.0,0.0,0.0,0.0 +0,1.287800,0.0,0.0,0.0,0.0,0.0 +0,1.287900,0.0,0.0,0.0,0.0,0.0 +0,1.288000,0.0,0.0,0.0,0.0,0.0 +0,1.288100,0.0,0.0,0.0,0.0,0.0 +0,1.288200,0.0,0.0,0.0,0.0,0.0 +0,1.288300,0.0,0.0,0.0,0.0,0.0 +0,1.288400,0.0,0.0,0.0,0.0,0.0 +0,1.288500,0.0,0.0,0.0,0.0,0.0 +0,1.288600,0.0,0.0,0.0,0.0,0.0 +0,1.288700,0.0,0.0,0.0,0.0,0.0 +0,1.288800,0.0,0.0,0.0,0.0,0.0 +0,1.288900,0.0,0.0,0.0,0.0,0.0 +0,1.289000,0.0,0.0,0.0,0.0,0.0 +0,1.289100,0.0,0.0,0.0,0.0,0.0 +0,1.289200,0.0,0.0,0.0,0.0,0.0 +0,1.289300,0.0,0.0,0.0,0.0,0.0 +0,1.289400,0.0,0.0,0.0,0.0,0.0 +0,1.289500,0.0,0.0,0.0,0.0,0.0 +0,1.289600,0.0,0.0,0.0,0.0,0.0 +0,1.289700,0.0,0.0,0.0,0.0,0.0 +0,1.289800,0.0,0.0,0.0,0.0,0.0 +0,1.289900,0.0,0.0,0.0,0.0,0.0 +0,1.290000,0.0,0.0,0.0,0.0,0.0 +0,1.290100,0.0,0.0,0.0,0.0,0.0 +0,1.290200,0.0,0.0,0.0,0.0,0.0 +0,1.290300,0.0,0.0,0.0,0.0,0.0 +0,1.290400,0.0,0.0,0.0,0.0,0.0 +0,1.290500,0.0,0.0,0.0,0.0,0.0 +0,1.290600,0.0,0.0,0.0,0.0,0.0 +0,1.290700,0.0,0.0,0.0,0.0,0.0 +0,1.290800,0.0,0.0,0.0,0.0,0.0 +0,1.290900,0.0,0.0,0.0,0.0,0.0 +0,1.291000,0.0,0.0,0.0,0.0,0.0 +0,1.291100,0.0,0.0,0.0,0.0,0.0 +0,1.291200,0.0,0.0,0.0,0.0,0.0 +0,1.291300,0.0,0.0,0.0,0.0,0.0 +0,1.291400,0.0,0.0,0.0,0.0,0.0 +0,1.291500,0.0,0.0,0.0,0.0,0.0 +0,1.291600,0.0,0.0,0.0,0.0,0.0 +0,1.291700,0.0,0.0,0.0,0.0,0.0 +0,1.291800,0.0,0.0,0.0,0.0,0.0 +0,1.291900,0.0,0.0,0.0,0.0,0.0 +0,1.292000,0.0,0.0,0.0,0.0,0.0 +0,1.292100,0.0,0.0,0.0,0.0,0.0 +0,1.292200,0.0,0.0,0.0,0.0,0.0 +0,1.292300,0.0,0.0,0.0,0.0,0.0 +0,1.292400,0.0,0.0,0.0,0.0,0.0 +0,1.292500,0.0,0.0,0.0,0.0,0.0 +0,1.292600,0.0,0.0,0.0,0.0,0.0 +0,1.292700,0.0,0.0,0.0,0.0,0.0 +0,1.292800,0.0,0.0,0.0,0.0,0.0 +0,1.292900,0.0,0.0,0.0,0.0,0.0 +0,1.293000,0.0,0.0,0.0,0.0,0.0 +0,1.293100,0.0,0.0,0.0,0.0,0.0 +0,1.293200,0.0,0.0,0.0,0.0,0.0 +0,1.293300,0.0,0.0,0.0,0.0,0.0 +0,1.293400,0.0,0.0,0.0,0.0,0.0 +0,1.293500,0.0,0.0,0.0,0.0,0.0 +0,1.293600,0.0,0.0,0.0,0.0,0.0 +0,1.293700,0.0,0.0,0.0,0.0,0.0 +0,1.293800,0.0,0.0,0.0,0.0,0.0 +0,1.293900,0.0,0.0,0.0,0.0,0.0 +0,1.294000,0.0,0.0,0.0,0.0,0.0 +0,1.294100,0.0,0.0,0.0,0.0,0.0 +0,1.294200,0.0,0.0,0.0,0.0,0.0 +0,1.294300,0.0,0.0,0.0,0.0,0.0 +0,1.294400,0.0,0.0,0.0,0.0,0.0 +0,1.294500,0.0,0.0,0.0,0.0,0.0 +0,1.294600,0.0,0.0,0.0,0.0,0.0 +0,1.294700,0.0,0.0,0.0,0.0,0.0 +0,1.294800,0.0,0.0,0.0,0.0,0.0 +0,1.294900,0.0,0.0,0.0,0.0,0.0 +0,1.295000,0.0,0.0,0.0,0.0,0.0 +0,1.295100,0.0,0.0,0.0,0.0,0.0 +0,1.295200,0.0,0.0,0.0,0.0,0.0 +0,1.295300,0.0,0.0,0.0,0.0,0.0 +0,1.295400,0.0,0.0,0.0,0.0,0.0 +0,1.295500,0.0,0.0,0.0,0.0,0.0 +0,1.295600,0.0,0.0,0.0,0.0,0.0 +0,1.295700,0.0,0.0,0.0,0.0,0.0 +0,1.295800,0.0,0.0,0.0,0.0,0.0 +0,1.295900,0.0,0.0,0.0,0.0,0.0 +0,1.296000,0.0,0.0,0.0,0.0,0.0 +0,1.296100,0.0,0.0,0.0,0.0,0.0 +0,1.296200,0.0,0.0,0.0,0.0,0.0 +0,1.296300,0.0,0.0,0.0,0.0,0.0 +0,1.296400,0.0,0.0,0.0,0.0,0.0 +0,1.296500,0.0,0.0,0.0,0.0,0.0 +0,1.296600,0.0,0.0,0.0,0.0,0.0 +0,1.296700,0.0,0.0,0.0,0.0,0.0 +0,1.296800,0.0,0.0,0.0,0.0,0.0 +0,1.296900,0.0,0.0,0.0,0.0,0.0 +0,1.297000,0.0,0.0,0.0,0.0,0.0 +0,1.297100,0.0,0.0,0.0,0.0,0.0 +0,1.297200,0.0,0.0,0.0,0.0,0.0 +0,1.297300,0.0,0.0,0.0,0.0,0.0 +0,1.297400,0.0,0.0,0.0,0.0,0.0 +0,1.297500,0.0,0.0,0.0,0.0,0.0 +0,1.297600,0.0,0.0,0.0,0.0,0.0 +0,1.297700,0.0,0.0,0.0,0.0,0.0 +0,1.297800,0.0,0.0,0.0,0.0,0.0 +0,1.297900,0.0,0.0,0.0,0.0,0.0 +0,1.298000,0.0,0.0,0.0,0.0,0.0 +0,1.298100,0.0,0.0,0.0,0.0,0.0 +0,1.298200,0.0,0.0,0.0,0.0,0.0 +0,1.298300,0.0,0.0,0.0,0.0,0.0 +0,1.298400,0.0,0.0,0.0,0.0,0.0 +0,1.298500,0.0,0.0,0.0,0.0,0.0 +0,1.298600,0.0,0.0,0.0,0.0,0.0 +0,1.298700,0.0,0.0,0.0,0.0,0.0 +0,1.298800,0.0,0.0,0.0,0.0,0.0 +0,1.298900,0.0,0.0,0.0,0.0,0.0 +0,1.299000,0.0,0.0,0.0,0.0,0.0 +0,1.299100,0.0,0.0,0.0,0.0,0.0 +0,1.299200,0.0,0.0,0.0,0.0,0.0 +0,1.299300,0.0,0.0,0.0,0.0,0.0 +0,1.299400,0.0,0.0,0.0,0.0,0.0 +0,1.299500,0.0,0.0,0.0,0.0,0.0 +0,1.299600,0.0,0.0,0.0,0.0,0.0 +0,1.299700,0.0,0.0,0.0,0.0,0.0 +0,1.299800,0.0,0.0,0.0,0.0,0.0 +0,1.299900,0.0,0.0,0.0,0.0,0.0 +0,1.300000,0.0,0.0,0.0,0.0,0.0 +0,1.300100,0.0,0.0,0.0,0.0,0.0 +1,915.733577,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.300200,0.0,0.0,0.0,0.0,0.0 +0,1.300300,0.0,0.0,0.0,0.0,0.0 +0,1.300400,0.0,0.0,0.0,0.0,0.0 +0,1.300500,0.0,0.0,0.0,0.0,0.0 +0,1.300600,0.0,0.0,0.0,0.0,0.0 +0,1.300700,0.0,0.0,0.0,0.0,0.0 +0,1.300800,0.0,0.0,0.0,0.0,0.0 +0,1.300900,0.0,0.0,0.0,0.0,0.0 +0,1.301000,0.0,0.0,0.0,0.0,0.0 +0,1.301100,0.0,0.0,0.0,0.0,0.0 +0,1.301200,0.0,0.0,0.0,0.0,0.0 +0,1.301300,0.0,0.0,0.0,0.0,0.0 +0,1.301400,0.0,0.0,0.0,0.0,0.0 +0,1.301500,0.0,0.0,0.0,0.0,0.0 +0,1.301600,0.0,0.0,0.0,0.0,0.0 +0,1.301700,0.0,0.0,0.0,0.0,0.0 +0,1.301800,0.0,0.0,0.0,0.0,0.0 +0,1.301900,0.0,0.0,0.0,0.0,0.0 +0,1.302000,0.0,0.0,0.0,0.0,0.0 +0,1.302100,0.0,0.0,0.0,0.0,0.0 +0,1.302200,0.0,0.0,0.0,0.0,0.0 +0,1.302300,0.0,0.0,0.0,0.0,0.0 +0,1.302400,0.0,0.0,0.0,0.0,0.0 +0,1.302500,0.0,0.0,0.0,0.0,0.0 +0,1.302600,0.0,0.0,0.0,0.0,0.0 +0,1.302700,0.0,0.0,0.0,0.0,0.0 +0,1.302800,0.0,0.0,0.0,0.0,0.0 +0,1.302900,0.0,0.0,0.0,0.0,0.0 +0,1.303000,0.0,0.0,0.0,0.0,0.0 +0,1.303100,0.0,0.0,0.0,0.0,0.0 +0,1.303200,0.0,0.0,0.0,0.0,0.0 +0,1.303300,0.0,0.0,0.0,0.0,0.0 +0,1.303400,0.0,0.0,0.0,0.0,0.0 +0,1.303500,0.0,0.0,0.0,0.0,0.0 +0,1.303600,0.0,0.0,0.0,0.0,0.0 +0,1.303700,0.0,0.0,0.0,0.0,0.0 +0,1.303800,0.0,0.0,0.0,0.0,0.0 +0,1.303900,0.0,0.0,0.0,0.0,0.0 +0,1.304000,0.0,0.0,0.0,0.0,0.0 +0,1.304100,0.0,0.0,0.0,0.0,0.0 +0,1.304200,0.0,0.0,0.0,0.0,0.0 +0,1.304300,0.0,0.0,0.0,0.0,0.0 +0,1.304400,0.0,0.0,0.0,0.0,0.0 +0,1.304500,0.0,0.0,0.0,0.0,0.0 +0,1.304600,0.0,0.0,0.0,0.0,0.0 +0,1.304700,0.0,0.0,0.0,0.0,0.0 +0,1.304800,0.0,0.0,0.0,0.0,0.0 +0,1.304900,0.0,0.0,0.0,0.0,0.0 +0,1.305000,0.0,0.0,0.0,0.0,0.0 +0,1.305100,0.0,0.0,0.0,0.0,0.0 +0,1.305200,0.0,0.0,0.0,0.0,0.0 +0,1.305300,0.0,0.0,0.0,0.0,0.0 +0,1.305400,0.0,0.0,0.0,0.0,0.0 +0,1.305500,0.0,0.0,0.0,0.0,0.0 +0,1.305600,0.0,0.0,0.0,0.0,0.0 +0,1.305700,0.0,0.0,0.0,0.0,0.0 +0,1.305800,0.0,0.0,0.0,0.0,0.0 +0,1.305900,0.0,0.0,0.0,0.0,0.0 +0,1.306000,0.0,0.0,0.0,0.0,0.0 +0,1.306100,0.0,0.0,0.0,0.0,0.0 +0,1.306200,0.0,0.0,0.0,0.0,0.0 +0,1.306300,0.0,0.0,0.0,0.0,0.0 +0,1.306400,0.0,0.0,0.0,0.0,0.0 +0,1.306500,0.0,0.0,0.0,0.0,0.0 +0,1.306600,0.0,0.0,0.0,0.0,0.0 +0,1.306700,0.0,0.0,0.0,0.0,0.0 +0,1.306800,0.0,0.0,0.0,0.0,0.0 +0,1.306900,0.0,0.0,0.0,0.0,0.0 +0,1.307000,0.0,0.0,0.0,0.0,0.0 +0,1.307100,0.0,0.0,0.0,0.0,0.0 +0,1.307200,0.0,0.0,0.0,0.0,0.0 +0,1.307300,0.0,0.0,0.0,0.0,0.0 +0,1.307400,0.0,0.0,0.0,0.0,0.0 +0,1.307500,0.0,0.0,0.0,0.0,0.0 +0,1.307600,0.0,0.0,0.0,0.0,0.0 +0,1.307700,0.0,0.0,0.0,0.0,0.0 +0,1.307800,0.0,0.0,0.0,0.0,0.0 +0,1.307900,0.0,0.0,0.0,0.0,0.0 +0,1.308000,0.0,0.0,0.0,0.0,0.0 +0,1.308100,0.0,0.0,0.0,0.0,0.0 +0,1.308200,0.0,0.0,0.0,0.0,0.0 +0,1.308300,0.0,0.0,0.0,0.0,0.0 +0,1.308400,0.0,0.0,0.0,0.0,0.0 +0,1.308500,0.0,0.0,0.0,0.0,0.0 +0,1.308600,0.0,0.0,0.0,0.0,0.0 +0,1.308700,0.0,0.0,0.0,0.0,0.0 +0,1.308800,0.0,0.0,0.0,0.0,0.0 +0,1.308900,0.0,0.0,0.0,0.0,0.0 +0,1.309000,0.0,0.0,0.0,0.0,0.0 +0,1.309100,0.0,0.0,0.0,0.0,0.0 +0,1.309200,0.0,0.0,0.0,0.0,0.0 +0,1.309300,0.0,0.0,0.0,0.0,0.0 +0,1.309400,0.0,0.0,0.0,0.0,0.0 +0,1.309500,0.0,0.0,0.0,0.0,0.0 +0,1.309600,0.0,0.0,0.0,0.0,0.0 +0,1.309700,0.0,0.0,0.0,0.0,0.0 +0,1.309800,0.0,0.0,0.0,0.0,0.0 +0,1.309900,0.0,0.0,0.0,0.0,0.0 +0,1.310000,0.0,0.0,0.0,0.0,0.0 +0,1.310100,0.0,0.0,0.0,0.0,0.0 +0,1.310200,0.0,0.0,0.0,0.0,0.0 +0,1.310300,0.0,0.0,0.0,0.0,0.0 +0,1.310400,0.0,0.0,0.0,0.0,0.0 +0,1.310500,0.0,0.0,0.0,0.0,0.0 +0,1.310600,0.0,0.0,0.0,0.0,0.0 +0,1.310700,0.0,0.0,0.0,0.0,0.0 +0,1.310800,0.0,0.0,0.0,0.0,0.0 +0,1.310900,0.0,0.0,0.0,0.0,0.0 +0,1.311000,0.0,0.0,0.0,0.0,0.0 +0,1.311100,0.0,0.0,0.0,0.0,0.0 +0,1.311200,0.0,0.0,0.0,0.0,0.0 +0,1.311300,0.0,0.0,0.0,0.0,0.0 +0,1.311400,0.0,0.0,0.0,0.0,0.0 +0,1.311500,0.0,0.0,0.0,0.0,0.0 +0,1.311600,0.0,0.0,0.0,0.0,0.0 +0,1.311700,0.0,0.0,0.0,0.0,0.0 +0,1.311800,0.0,0.0,0.0,0.0,0.0 +0,1.311900,0.0,0.0,0.0,0.0,0.0 +0,1.312000,0.0,0.0,0.0,0.0,0.0 +0,1.312100,0.0,0.0,0.0,0.0,0.0 +0,1.312200,0.0,0.0,0.0,0.0,0.0 +0,1.312300,0.0,0.0,0.0,0.0,0.0 +0,1.312400,0.0,0.0,0.0,0.0,0.0 +0,1.312500,0.0,0.0,0.0,0.0,0.0 +0,1.312600,0.0,0.0,0.0,0.0,0.0 +0,1.312700,0.0,0.0,0.0,0.0,0.0 +0,1.312800,0.0,0.0,0.0,0.0,0.0 +0,1.312900,0.0,0.0,0.0,0.0,0.0 +0,1.313000,0.0,0.0,0.0,0.0,0.0 +0,1.313100,0.0,0.0,0.0,0.0,0.0 +0,1.313200,0.0,0.0,0.0,0.0,0.0 +0,1.313300,0.0,0.0,0.0,0.0,0.0 +0,1.313400,0.0,0.0,0.0,0.0,0.0 +0,1.313500,0.0,0.0,0.0,0.0,0.0 +0,1.313600,0.0,0.0,0.0,0.0,0.0 +0,1.313700,0.0,0.0,0.0,0.0,0.0 +0,1.313800,0.0,0.0,0.0,0.0,0.0 +0,1.313900,0.0,0.0,0.0,0.0,0.0 +0,1.314000,0.0,0.0,0.0,0.0,0.0 +0,1.314100,0.0,0.0,0.0,0.0,0.0 +0,1.314200,0.0,0.0,0.0,0.0,0.0 +0,1.314300,0.0,0.0,0.0,0.0,0.0 +0,1.314400,0.0,0.0,0.0,0.0,0.0 +0,1.314500,0.0,0.0,0.0,0.0,0.0 +0,1.314600,0.0,0.0,0.0,0.0,0.0 +0,1.314700,0.0,0.0,0.0,0.0,0.0 +0,1.314800,0.0,0.0,0.0,0.0,0.0 +0,1.314900,0.0,0.0,0.0,0.0,0.0 +0,1.315000,0.0,0.0,0.0,0.0,0.0 +0,1.315100,0.0,0.0,0.0,0.0,0.0 +0,1.315200,0.0,0.0,0.0,0.0,0.0 +0,1.315300,0.0,0.0,0.0,0.0,0.0 +0,1.315400,0.0,0.0,0.0,0.0,0.0 +0,1.315500,0.0,0.0,0.0,0.0,0.0 +0,1.315600,0.0,0.0,0.0,0.0,0.0 +0,1.315700,0.0,0.0,0.0,0.0,0.0 +0,1.315800,0.0,0.0,0.0,0.0,0.0 +0,1.315900,0.0,0.0,0.0,0.0,0.0 +0,1.316000,0.0,0.0,0.0,0.0,0.0 +0,1.316100,0.0,0.0,0.0,0.0,0.0 +0,1.316200,0.0,0.0,0.0,0.0,0.0 +0,1.316300,0.0,0.0,0.0,0.0,0.0 +0,1.316400,0.0,0.0,0.0,0.0,0.0 +0,1.316500,0.0,0.0,0.0,0.0,0.0 +0,1.316600,0.0,0.0,0.0,0.0,0.0 +0,1.316700,0.0,0.0,0.0,0.0,0.0 +0,1.316800,0.0,0.0,0.0,0.0,0.0 +0,1.316900,0.0,0.0,0.0,0.0,0.0 +0,1.317000,0.0,0.0,0.0,0.0,0.0 +0,1.317100,0.0,0.0,0.0,0.0,0.0 +0,1.317200,0.0,0.0,0.0,0.0,0.0 +0,1.317300,0.0,0.0,0.0,0.0,0.0 +0,1.317400,0.0,0.0,0.0,0.0,0.0 +0,1.317500,0.0,0.0,0.0,0.0,0.0 +0,1.317600,0.0,0.0,0.0,0.0,0.0 +0,1.317700,0.0,0.0,0.0,0.0,0.0 +0,1.317800,0.0,0.0,0.0,0.0,0.0 +0,1.317900,0.0,0.0,0.0,0.0,0.0 +0,1.318000,0.0,0.0,0.0,0.0,0.0 +0,1.318100,0.0,0.0,0.0,0.0,0.0 +0,1.318200,0.0,0.0,0.0,0.0,0.0 +0,1.318300,0.0,0.0,0.0,0.0,0.0 +0,1.318400,0.0,0.0,0.0,0.0,0.0 +0,1.318500,0.0,0.0,0.0,0.0,0.0 +0,1.318600,0.0,0.0,0.0,0.0,0.0 +0,1.318700,0.0,0.0,0.0,0.0,0.0 +0,1.318800,0.0,0.0,0.0,0.0,0.0 +0,1.318900,0.0,0.0,0.0,0.0,0.0 +0,1.319000,0.0,0.0,0.0,0.0,0.0 +0,1.319100,0.0,0.0,0.0,0.0,0.0 +0,1.319200,0.0,0.0,0.0,0.0,0.0 +0,1.319300,0.0,0.0,0.0,0.0,0.0 +0,1.319400,0.0,0.0,0.0,0.0,0.0 +0,1.319500,0.0,0.0,0.0,0.0,0.0 +0,1.319600,0.0,0.0,0.0,0.0,0.0 +0,1.319700,0.0,0.0,0.0,0.0,0.0 +0,1.319800,0.0,0.0,0.0,0.0,0.0 +0,1.319900,0.0,0.0,0.0,0.0,0.0 +0,1.320000,0.0,0.0,0.0,0.0,0.0 +0,1.320100,0.0,0.0,0.0,0.0,0.0 +1,958.646736,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.320200,0.0,0.0,0.0,0.0,0.0 +0,1.320300,0.0,0.0,0.0,0.0,0.0 +0,1.320400,0.0,0.0,0.0,0.0,0.0 +0,1.320500,0.0,0.0,0.0,0.0,0.0 +0,1.320600,0.0,0.0,0.0,0.0,0.0 +0,1.320700,0.0,0.0,0.0,0.0,0.0 +0,1.320800,0.0,0.0,0.0,0.0,0.0 +0,1.320900,0.0,0.0,0.0,0.0,0.0 +0,1.321000,0.0,0.0,0.0,0.0,0.0 +0,1.321100,0.0,0.0,0.0,0.0,0.0 +0,1.321200,0.0,0.0,0.0,0.0,0.0 +0,1.321300,0.0,0.0,0.0,0.0,0.0 +0,1.321400,0.0,0.0,0.0,0.0,0.0 +0,1.321500,0.0,0.0,0.0,0.0,0.0 +0,1.321600,0.0,0.0,0.0,0.0,0.0 +0,1.321700,0.0,0.0,0.0,0.0,0.0 +0,1.321800,0.0,0.0,0.0,0.0,0.0 +0,1.321900,0.0,0.0,0.0,0.0,0.0 +0,1.322000,0.0,0.0,0.0,0.0,0.0 +0,1.322100,0.0,0.0,0.0,0.0,0.0 +0,1.322200,0.0,0.0,0.0,0.0,0.0 +0,1.322300,0.0,0.0,0.0,0.0,0.0 +0,1.322400,0.0,0.0,0.0,0.0,0.0 +0,1.322500,0.0,0.0,0.0,0.0,0.0 +0,1.322600,0.0,0.0,0.0,0.0,0.0 +0,1.322700,0.0,0.0,0.0,0.0,0.0 +0,1.322800,0.0,0.0,0.0,0.0,0.0 +0,1.322900,0.0,0.0,0.0,0.0,0.0 +0,1.323000,0.0,0.0,0.0,0.0,0.0 +0,1.323100,0.0,0.0,0.0,0.0,0.0 +0,1.323200,0.0,0.0,0.0,0.0,0.0 +0,1.323300,0.0,0.0,0.0,0.0,0.0 +0,1.323400,0.0,0.0,0.0,0.0,0.0 +0,1.323500,0.0,0.0,0.0,0.0,0.0 +0,1.323600,0.0,0.0,0.0,0.0,0.0 +0,1.323700,0.0,0.0,0.0,0.0,0.0 +0,1.323800,0.0,0.0,0.0,0.0,0.0 +0,1.323900,0.0,0.0,0.0,0.0,0.0 +0,1.324000,0.0,0.0,0.0,0.0,0.0 +0,1.324100,0.0,0.0,0.0,0.0,0.0 +0,1.324200,0.0,0.0,0.0,0.0,0.0 +0,1.324300,0.0,0.0,0.0,0.0,0.0 +0,1.324400,0.0,0.0,0.0,0.0,0.0 +0,1.324500,0.0,0.0,0.0,0.0,0.0 +0,1.324600,0.0,0.0,0.0,0.0,0.0 +0,1.324700,0.0,0.0,0.0,0.0,0.0 +0,1.324800,0.0,0.0,0.0,0.0,0.0 +0,1.324900,0.0,0.0,0.0,0.0,0.0 +0,1.325000,0.0,0.0,0.0,0.0,0.0 +0,1.325100,0.0,0.0,0.0,0.0,0.0 +0,1.325200,0.0,0.0,0.0,0.0,0.0 +0,1.325300,0.0,0.0,0.0,0.0,0.0 +0,1.325400,0.0,0.0,0.0,0.0,0.0 +0,1.325500,0.0,0.0,0.0,0.0,0.0 +0,1.325600,0.0,0.0,0.0,0.0,0.0 +0,1.325700,0.0,0.0,0.0,0.0,0.0 +0,1.325800,0.0,0.0,0.0,0.0,0.0 +0,1.325900,0.0,0.0,0.0,0.0,0.0 +0,1.326000,0.0,0.0,0.0,0.0,0.0 +0,1.326100,0.0,0.0,0.0,0.0,0.0 +0,1.326200,0.0,0.0,0.0,0.0,0.0 +0,1.326300,0.0,0.0,0.0,0.0,0.0 +0,1.326400,0.0,0.0,0.0,0.0,0.0 +0,1.326500,0.0,0.0,0.0,0.0,0.0 +0,1.326600,0.0,0.0,0.0,0.0,0.0 +0,1.326700,0.0,0.0,0.0,0.0,0.0 +0,1.326800,0.0,0.0,0.0,0.0,0.0 +0,1.326900,0.0,0.0,0.0,0.0,0.0 +0,1.327000,0.0,0.0,0.0,0.0,0.0 +0,1.327100,0.0,0.0,0.0,0.0,0.0 +0,1.327200,0.0,0.0,0.0,0.0,0.0 +0,1.327300,0.0,0.0,0.0,0.0,0.0 +0,1.327400,0.0,0.0,0.0,0.0,0.0 +0,1.327500,0.0,0.0,0.0,0.0,0.0 +0,1.327600,0.0,0.0,0.0,0.0,0.0 +0,1.327700,0.0,0.0,0.0,0.0,0.0 +0,1.327800,0.0,0.0,0.0,0.0,0.0 +0,1.327900,0.0,0.0,0.0,0.0,0.0 +0,1.328000,0.0,0.0,0.0,0.0,0.0 +0,1.328100,0.0,0.0,0.0,0.0,0.0 +0,1.328200,0.0,0.0,0.0,0.0,0.0 +0,1.328300,0.0,0.0,0.0,0.0,0.0 +0,1.328400,0.0,0.0,0.0,0.0,0.0 +0,1.328500,0.0,0.0,0.0,0.0,0.0 +0,1.328600,0.0,0.0,0.0,0.0,0.0 +0,1.328700,0.0,0.0,0.0,0.0,0.0 +0,1.328800,0.0,0.0,0.0,0.0,0.0 +0,1.328900,0.0,0.0,0.0,0.0,0.0 +0,1.329000,0.0,0.0,0.0,0.0,0.0 +0,1.329100,0.0,0.0,0.0,0.0,0.0 +0,1.329200,0.0,0.0,0.0,0.0,0.0 +0,1.329300,0.0,0.0,0.0,0.0,0.0 +0,1.329400,0.0,0.0,0.0,0.0,0.0 +0,1.329500,0.0,0.0,0.0,0.0,0.0 +0,1.329600,0.0,0.0,0.0,0.0,0.0 +0,1.329700,0.0,0.0,0.0,0.0,0.0 +0,1.329800,0.0,0.0,0.0,0.0,0.0 +0,1.329900,0.0,0.0,0.0,0.0,0.0 +0,1.330000,0.0,0.0,0.0,0.0,0.0 +0,1.330100,0.0,0.0,0.0,0.0,0.0 +0,1.330200,0.0,0.0,0.0,0.0,0.0 +0,1.330300,0.0,0.0,0.0,0.0,0.0 +0,1.330400,0.0,0.0,0.0,0.0,0.0 +0,1.330500,0.0,0.0,0.0,0.0,0.0 +0,1.330600,0.0,0.0,0.0,0.0,0.0 +0,1.330700,0.0,0.0,0.0,0.0,0.0 +0,1.330800,0.0,0.0,0.0,0.0,0.0 +0,1.330900,0.0,0.0,0.0,0.0,0.0 +0,1.331000,0.0,0.0,0.0,0.0,0.0 +0,1.331100,0.0,0.0,0.0,0.0,0.0 +0,1.331200,0.0,0.0,0.0,0.0,0.0 +0,1.331300,0.0,0.0,0.0,0.0,0.0 +0,1.331400,0.0,0.0,0.0,0.0,0.0 +0,1.331500,0.0,0.0,0.0,0.0,0.0 +0,1.331600,0.0,0.0,0.0,0.0,0.0 +0,1.331700,0.0,0.0,0.0,0.0,0.0 +0,1.331800,0.0,0.0,0.0,0.0,0.0 +0,1.331900,0.0,0.0,0.0,0.0,0.0 +0,1.332000,0.0,0.0,0.0,0.0,0.0 +0,1.332100,0.0,0.0,0.0,0.0,0.0 +0,1.332200,0.0,0.0,0.0,0.0,0.0 +0,1.332300,0.0,0.0,0.0,0.0,0.0 +0,1.332400,0.0,0.0,0.0,0.0,0.0 +0,1.332500,0.0,0.0,0.0,0.0,0.0 +0,1.332600,0.0,0.0,0.0,0.0,0.0 +0,1.332700,0.0,0.0,0.0,0.0,0.0 +0,1.332800,0.0,0.0,0.0,0.0,0.0 +0,1.332900,0.0,0.0,0.0,0.0,0.0 +0,1.333000,0.0,0.0,0.0,0.0,0.0 +0,1.333100,0.0,0.0,0.0,0.0,0.0 +0,1.333200,0.0,0.0,0.0,0.0,0.0 +0,1.333300,0.0,0.0,0.0,0.0,0.0 +0,1.333400,0.0,0.0,0.0,0.0,0.0 +0,1.333500,0.0,0.0,0.0,0.0,0.0 +0,1.333600,0.0,0.0,0.0,0.0,0.0 +0,1.333700,0.0,0.0,0.0,0.0,0.0 +0,1.333800,0.0,0.0,0.0,0.0,0.0 +0,1.333900,0.0,0.0,0.0,0.0,0.0 +0,1.334000,0.0,0.0,0.0,0.0,0.0 +0,1.334100,0.0,0.0,0.0,0.0,0.0 +0,1.334200,0.0,0.0,0.0,0.0,0.0 +0,1.334300,0.0,0.0,0.0,0.0,0.0 +0,1.334400,0.0,0.0,0.0,0.0,0.0 +0,1.334500,0.0,0.0,0.0,0.0,0.0 +0,1.334600,0.0,0.0,0.0,0.0,0.0 +0,1.334700,0.0,0.0,0.0,0.0,0.0 +0,1.334800,0.0,0.0,0.0,0.0,0.0 +0,1.334900,0.0,0.0,0.0,0.0,0.0 +0,1.335000,0.0,0.0,0.0,0.0,0.0 +0,1.335100,0.0,0.0,0.0,0.0,0.0 +0,1.335200,0.0,0.0,0.0,0.0,0.0 +0,1.335300,0.0,0.0,0.0,0.0,0.0 +0,1.335400,0.0,0.0,0.0,0.0,0.0 +0,1.335500,0.0,0.0,0.0,0.0,0.0 +0,1.335600,0.0,0.0,0.0,0.0,0.0 +0,1.335700,0.0,0.0,0.0,0.0,0.0 +0,1.335800,0.0,0.0,0.0,0.0,0.0 +0,1.335900,0.0,0.0,0.0,0.0,0.0 +0,1.336000,0.0,0.0,0.0,0.0,0.0 +0,1.336100,0.0,0.0,0.0,0.0,0.0 +0,1.336200,0.0,0.0,0.0,0.0,0.0 +0,1.336300,0.0,0.0,0.0,0.0,0.0 +0,1.336400,0.0,0.0,0.0,0.0,0.0 +0,1.336500,0.0,0.0,0.0,0.0,0.0 +0,1.336600,0.0,0.0,0.0,0.0,0.0 +0,1.336700,0.0,0.0,0.0,0.0,0.0 +0,1.336800,0.0,0.0,0.0,0.0,0.0 +0,1.336900,0.0,0.0,0.0,0.0,0.0 +0,1.337000,0.0,0.0,0.0,0.0,0.0 +0,1.337100,0.0,0.0,0.0,0.0,0.0 +0,1.337200,0.0,0.0,0.0,0.0,0.0 +0,1.337300,0.0,0.0,0.0,0.0,0.0 +0,1.337400,0.0,0.0,0.0,0.0,0.0 +0,1.337500,0.0,0.0,0.0,0.0,0.0 +0,1.337600,0.0,0.0,0.0,0.0,0.0 +0,1.337700,0.0,0.0,0.0,0.0,0.0 +0,1.337800,0.0,0.0,0.0,0.0,0.0 +0,1.337900,0.0,0.0,0.0,0.0,0.0 +0,1.338000,0.0,0.0,0.0,0.0,0.0 +0,1.338100,0.0,0.0,0.0,0.0,0.0 +0,1.338200,0.0,0.0,0.0,0.0,0.0 +0,1.338300,0.0,0.0,0.0,0.0,0.0 +0,1.338400,0.0,0.0,0.0,0.0,0.0 +0,1.338500,0.0,0.0,0.0,0.0,0.0 +0,1.338600,0.0,0.0,0.0,0.0,0.0 +0,1.338700,0.0,0.0,0.0,0.0,0.0 +0,1.338800,0.0,0.0,0.0,0.0,0.0 +0,1.338900,0.0,0.0,0.0,0.0,0.0 +0,1.339000,0.0,0.0,0.0,0.0,0.0 +0,1.339100,0.0,0.0,0.0,0.0,0.0 +0,1.339200,0.0,0.0,0.0,0.0,0.0 +0,1.339300,0.0,0.0,0.0,0.0,0.0 +0,1.339400,0.0,0.0,0.0,0.0,0.0 +0,1.339500,0.0,0.0,0.0,0.0,0.0 +0,1.339600,0.0,0.0,0.0,0.0,0.0 +0,1.339700,0.0,0.0,0.0,0.0,0.0 +0,1.339800,0.0,0.0,0.0,0.0,0.0 +0,1.339900,0.0,0.0,0.0,0.0,0.0 +0,1.340000,0.0,0.0,0.0,0.0,0.0 +0,1.340100,0.0,0.0,0.0,0.0,0.0 +1,1002.880045,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.340200,0.0,0.0,0.0,0.0,0.0 +0,1.340300,0.0,0.0,0.0,0.0,0.0 +0,1.340400,0.0,0.0,0.0,0.0,0.0 +0,1.340500,0.0,0.0,0.0,0.0,0.0 +0,1.340600,0.0,0.0,0.0,0.0,0.0 +0,1.340700,0.0,0.0,0.0,0.0,0.0 +0,1.340800,0.0,0.0,0.0,0.0,0.0 +0,1.340900,0.0,0.0,0.0,0.0,0.0 +0,1.341000,0.0,0.0,0.0,0.0,0.0 +0,1.341100,0.0,0.0,0.0,0.0,0.0 +0,1.341200,0.0,0.0,0.0,0.0,0.0 +0,1.341300,0.0,0.0,0.0,0.0,0.0 +0,1.341400,0.0,0.0,0.0,0.0,0.0 +0,1.341500,0.0,0.0,0.0,0.0,0.0 +0,1.341600,0.0,0.0,0.0,0.0,0.0 +0,1.341700,0.0,0.0,0.0,0.0,0.0 +0,1.341800,0.0,0.0,0.0,0.0,0.0 +0,1.341900,0.0,0.0,0.0,0.0,0.0 +0,1.342000,0.0,0.0,0.0,0.0,0.0 +0,1.342100,0.0,0.0,0.0,0.0,0.0 +0,1.342200,0.0,0.0,0.0,0.0,0.0 +0,1.342300,0.0,0.0,0.0,0.0,0.0 +0,1.342400,0.0,0.0,0.0,0.0,0.0 +0,1.342500,0.0,0.0,0.0,0.0,0.0 +0,1.342600,0.0,0.0,0.0,0.0,0.0 +0,1.342700,0.0,0.0,0.0,0.0,0.0 +0,1.342800,0.0,0.0,0.0,0.0,0.0 +0,1.342900,0.0,0.0,0.0,0.0,0.0 +0,1.343000,0.0,0.0,0.0,0.0,0.0 +0,1.343100,0.0,0.0,0.0,0.0,0.0 +0,1.343200,0.0,0.0,0.0,0.0,0.0 +0,1.343300,0.0,0.0,0.0,0.0,0.0 +0,1.343400,0.0,0.0,0.0,0.0,0.0 +0,1.343500,0.0,0.0,0.0,0.0,0.0 +0,1.343600,0.0,0.0,0.0,0.0,0.0 +0,1.343700,0.0,0.0,0.0,0.0,0.0 +0,1.343800,0.0,0.0,0.0,0.0,0.0 +0,1.343900,0.0,0.0,0.0,0.0,0.0 +0,1.344000,0.0,0.0,0.0,0.0,0.0 +0,1.344100,0.0,0.0,0.0,0.0,0.0 +0,1.344200,0.0,0.0,0.0,0.0,0.0 +0,1.344300,0.0,0.0,0.0,0.0,0.0 +0,1.344400,0.0,0.0,0.0,0.0,0.0 +0,1.344500,0.0,0.0,0.0,0.0,0.0 +0,1.344600,0.0,0.0,0.0,0.0,0.0 +0,1.344700,0.0,0.0,0.0,0.0,0.0 +0,1.344800,0.0,0.0,0.0,0.0,0.0 +0,1.344900,0.0,0.0,0.0,0.0,0.0 +0,1.345000,0.0,0.0,0.0,0.0,0.0 +0,1.345100,0.0,0.0,0.0,0.0,0.0 +0,1.345200,0.0,0.0,0.0,0.0,0.0 +0,1.345300,0.0,0.0,0.0,0.0,0.0 +0,1.345400,0.0,0.0,0.0,0.0,0.0 +0,1.345500,0.0,0.0,0.0,0.0,0.0 +0,1.345600,0.0,0.0,0.0,0.0,0.0 +0,1.345700,0.0,0.0,0.0,0.0,0.0 +0,1.345800,0.0,0.0,0.0,0.0,0.0 +0,1.345900,0.0,0.0,0.0,0.0,0.0 +0,1.346000,0.0,0.0,0.0,0.0,0.0 +0,1.346100,0.0,0.0,0.0,0.0,0.0 +0,1.346200,0.0,0.0,0.0,0.0,0.0 +0,1.346300,0.0,0.0,0.0,0.0,0.0 +0,1.346400,0.0,0.0,0.0,0.0,0.0 +0,1.346500,0.0,0.0,0.0,0.0,0.0 +0,1.346600,0.0,0.0,0.0,0.0,0.0 +0,1.346700,0.0,0.0,0.0,0.0,0.0 +0,1.346800,0.0,0.0,0.0,0.0,0.0 +0,1.346900,0.0,0.0,0.0,0.0,0.0 +0,1.347000,0.0,0.0,0.0,0.0,0.0 +0,1.347100,0.0,0.0,0.0,0.0,0.0 +0,1.347200,0.0,0.0,0.0,0.0,0.0 +0,1.347300,0.0,0.0,0.0,0.0,0.0 +0,1.347400,0.0,0.0,0.0,0.0,0.0 +0,1.347500,0.0,0.0,0.0,0.0,0.0 +0,1.347600,0.0,0.0,0.0,0.0,0.0 +0,1.347700,0.0,0.0,0.0,0.0,0.0 +0,1.347800,0.0,0.0,0.0,0.0,0.0 +0,1.347900,0.0,0.0,0.0,0.0,0.0 +0,1.348000,0.0,0.0,0.0,0.0,0.0 +0,1.348100,0.0,0.0,0.0,0.0,0.0 +0,1.348200,0.0,0.0,0.0,0.0,0.0 +0,1.348300,0.0,0.0,0.0,0.0,0.0 +0,1.348400,0.0,0.0,0.0,0.0,0.0 +0,1.348500,0.0,0.0,0.0,0.0,0.0 +0,1.348600,0.0,0.0,0.0,0.0,0.0 +0,1.348700,0.0,0.0,0.0,0.0,0.0 +0,1.348800,0.0,0.0,0.0,0.0,0.0 +0,1.348900,0.0,0.0,0.0,0.0,0.0 +0,1.349000,0.0,0.0,0.0,0.0,0.0 +0,1.349100,0.0,0.0,0.0,0.0,0.0 +0,1.349200,0.0,0.0,0.0,0.0,0.0 +0,1.349300,0.0,0.0,0.0,0.0,0.0 +0,1.349400,0.0,0.0,0.0,0.0,0.0 +0,1.349500,0.0,0.0,0.0,0.0,0.0 +0,1.349600,0.0,0.0,0.0,0.0,0.0 +0,1.349700,0.0,0.0,0.0,0.0,0.0 +0,1.349800,0.0,0.0,0.0,0.0,0.0 +0,1.349900,0.0,0.0,0.0,0.0,0.0 +0,1.350000,0.0,0.0,0.0,0.0,0.0 +0,1.350100,0.0,0.0,0.0,0.0,0.0 +0,1.350200,0.0,0.0,0.0,0.0,0.0 +0,1.350300,0.0,0.0,0.0,0.0,0.0 +0,1.350400,0.0,0.0,0.0,0.0,0.0 +0,1.350500,0.0,0.0,0.0,0.0,0.0 +0,1.350600,0.0,0.0,0.0,0.0,0.0 +0,1.350700,0.0,0.0,0.0,0.0,0.0 +0,1.350800,0.0,0.0,0.0,0.0,0.0 +0,1.350900,0.0,0.0,0.0,0.0,0.0 +0,1.351000,0.0,0.0,0.0,0.0,0.0 +0,1.351100,0.0,0.0,0.0,0.0,0.0 +0,1.351200,0.0,0.0,0.0,0.0,0.0 +0,1.351300,0.0,0.0,0.0,0.0,0.0 +0,1.351400,0.0,0.0,0.0,0.0,0.0 +0,1.351500,0.0,0.0,0.0,0.0,0.0 +0,1.351600,0.0,0.0,0.0,0.0,0.0 +0,1.351700,0.0,0.0,0.0,0.0,0.0 +0,1.351800,0.0,0.0,0.0,0.0,0.0 +0,1.351900,0.0,0.0,0.0,0.0,0.0 +0,1.352000,0.0,0.0,0.0,0.0,0.0 +0,1.352100,0.0,0.0,0.0,0.0,0.0 +0,1.352200,0.0,0.0,0.0,0.0,0.0 +0,1.352300,0.0,0.0,0.0,0.0,0.0 +0,1.352400,0.0,0.0,0.0,0.0,0.0 +0,1.352500,0.0,0.0,0.0,0.0,0.0 +0,1.352600,0.0,0.0,0.0,0.0,0.0 +0,1.352700,0.0,0.0,0.0,0.0,0.0 +0,1.352800,0.0,0.0,0.0,0.0,0.0 +0,1.352900,0.0,0.0,0.0,0.0,0.0 +0,1.353000,0.0,0.0,0.0,0.0,0.0 +0,1.353100,0.0,0.0,0.0,0.0,0.0 +0,1.353200,0.0,0.0,0.0,0.0,0.0 +0,1.353300,0.0,0.0,0.0,0.0,0.0 +0,1.353400,0.0,0.0,0.0,0.0,0.0 +0,1.353500,0.0,0.0,0.0,0.0,0.0 +0,1.353600,0.0,0.0,0.0,0.0,0.0 +0,1.353700,0.0,0.0,0.0,0.0,0.0 +0,1.353800,0.0,0.0,0.0,0.0,0.0 +0,1.353900,0.0,0.0,0.0,0.0,0.0 +0,1.354000,0.0,0.0,0.0,0.0,0.0 +0,1.354100,0.0,0.0,0.0,0.0,0.0 +0,1.354200,0.0,0.0,0.0,0.0,0.0 +0,1.354300,0.0,0.0,0.0,0.0,0.0 +0,1.354400,0.0,0.0,0.0,0.0,0.0 +0,1.354500,0.0,0.0,0.0,0.0,0.0 +0,1.354600,0.0,0.0,0.0,0.0,0.0 +0,1.354700,0.0,0.0,0.0,0.0,0.0 +0,1.354800,0.0,0.0,0.0,0.0,0.0 +0,1.354900,0.0,0.0,0.0,0.0,0.0 +0,1.355000,0.0,0.0,0.0,0.0,0.0 +0,1.355100,0.0,0.0,0.0,0.0,0.0 +0,1.355200,0.0,0.0,0.0,0.0,0.0 +0,1.355300,0.0,0.0,0.0,0.0,0.0 +0,1.355400,0.0,0.0,0.0,0.0,0.0 +0,1.355500,0.0,0.0,0.0,0.0,0.0 +0,1.355600,0.0,0.0,0.0,0.0,0.0 +0,1.355700,0.0,0.0,0.0,0.0,0.0 +0,1.355800,0.0,0.0,0.0,0.0,0.0 +0,1.355900,0.0,0.0,0.0,0.0,0.0 +0,1.356000,0.0,0.0,0.0,0.0,0.0 +0,1.356100,0.0,0.0,0.0,0.0,0.0 +0,1.356200,0.0,0.0,0.0,0.0,0.0 +0,1.356300,0.0,0.0,0.0,0.0,0.0 +0,1.356400,0.0,0.0,0.0,0.0,0.0 +0,1.356500,0.0,0.0,0.0,0.0,0.0 +0,1.356600,0.0,0.0,0.0,0.0,0.0 +0,1.356700,0.0,0.0,0.0,0.0,0.0 +0,1.356800,0.0,0.0,0.0,0.0,0.0 +0,1.356900,0.0,0.0,0.0,0.0,0.0 +0,1.357000,0.0,0.0,0.0,0.0,0.0 +0,1.357100,0.0,0.0,0.0,0.0,0.0 +0,1.357200,0.0,0.0,0.0,0.0,0.0 +0,1.357300,0.0,0.0,0.0,0.0,0.0 +0,1.357400,0.0,0.0,0.0,0.0,0.0 +0,1.357500,0.0,0.0,0.0,0.0,0.0 +0,1.357600,0.0,0.0,0.0,0.0,0.0 +0,1.357700,0.0,0.0,0.0,0.0,0.0 +0,1.357800,0.0,0.0,0.0,0.0,0.0 +0,1.357900,0.0,0.0,0.0,0.0,0.0 +0,1.358000,0.0,0.0,0.0,0.0,0.0 +0,1.358100,0.0,0.0,0.0,0.0,0.0 +0,1.358200,0.0,0.0,0.0,0.0,0.0 +0,1.358300,0.0,0.0,0.0,0.0,0.0 +0,1.358400,0.0,0.0,0.0,0.0,0.0 +0,1.358500,0.0,0.0,0.0,0.0,0.0 +0,1.358600,0.0,0.0,0.0,0.0,0.0 +0,1.358700,0.0,0.0,0.0,0.0,0.0 +0,1.358800,0.0,0.0,0.0,0.0,0.0 +0,1.358900,0.0,0.0,0.0,0.0,0.0 +0,1.359000,0.0,0.0,0.0,0.0,0.0 +0,1.359100,0.0,0.0,0.0,0.0,0.0 +0,1.359200,0.0,0.0,0.0,0.0,0.0 +0,1.359300,0.0,0.0,0.0,0.0,0.0 +0,1.359400,0.0,0.0,0.0,0.0,0.0 +0,1.359500,0.0,0.0,0.0,0.0,0.0 +0,1.359600,0.0,0.0,0.0,0.0,0.0 +0,1.359700,0.0,0.0,0.0,0.0,0.0 +0,1.359800,0.0,0.0,0.0,0.0,0.0 +0,1.359900,0.0,0.0,0.0,0.0,0.0 +0,1.360000,0.0,0.0,0.0,0.0,0.0 +0,1.360100,0.0,0.0,0.0,0.0,0.0 +1,1048.453504,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.360200,0.0,0.0,0.0,0.0,0.0 +0,1.360300,0.0,0.0,0.0,0.0,0.0 +0,1.360400,0.0,0.0,0.0,0.0,0.0 +0,1.360500,0.0,0.0,0.0,0.0,0.0 +0,1.360600,0.0,0.0,0.0,0.0,0.0 +0,1.360700,0.0,0.0,0.0,0.0,0.0 +0,1.360800,0.0,0.0,0.0,0.0,0.0 +0,1.360900,0.0,0.0,0.0,0.0,0.0 +0,1.361000,0.0,0.0,0.0,0.0,0.0 +0,1.361100,0.0,0.0,0.0,0.0,0.0 +0,1.361200,0.0,0.0,0.0,0.0,0.0 +0,1.361300,0.0,0.0,0.0,0.0,0.0 +0,1.361400,0.0,0.0,0.0,0.0,0.0 +0,1.361500,0.0,0.0,0.0,0.0,0.0 +0,1.361600,0.0,0.0,0.0,0.0,0.0 +0,1.361700,0.0,0.0,0.0,0.0,0.0 +0,1.361800,0.0,0.0,0.0,0.0,0.0 +0,1.361900,0.0,0.0,0.0,0.0,0.0 +0,1.362000,0.0,0.0,0.0,0.0,0.0 +0,1.362100,0.0,0.0,0.0,0.0,0.0 +0,1.362200,0.0,0.0,0.0,0.0,0.0 +0,1.362300,0.0,0.0,0.0,0.0,0.0 +0,1.362400,0.0,0.0,0.0,0.0,0.0 +0,1.362500,0.0,0.0,0.0,0.0,0.0 +0,1.362600,0.0,0.0,0.0,0.0,0.0 +0,1.362700,0.0,0.0,0.0,0.0,0.0 +0,1.362800,0.0,0.0,0.0,0.0,0.0 +0,1.362900,0.0,0.0,0.0,0.0,0.0 +0,1.363000,0.0,0.0,0.0,0.0,0.0 +0,1.363100,0.0,0.0,0.0,0.0,0.0 +0,1.363200,0.0,0.0,0.0,0.0,0.0 +0,1.363300,0.0,0.0,0.0,0.0,0.0 +0,1.363400,0.0,0.0,0.0,0.0,0.0 +0,1.363500,0.0,0.0,0.0,0.0,0.0 +0,1.363600,0.0,0.0,0.0,0.0,0.0 +0,1.363700,0.0,0.0,0.0,0.0,0.0 +0,1.363800,0.0,0.0,0.0,0.0,0.0 +0,1.363900,0.0,0.0,0.0,0.0,0.0 +0,1.364000,0.0,0.0,0.0,0.0,0.0 +0,1.364100,0.0,0.0,0.0,0.0,0.0 +0,1.364200,0.0,0.0,0.0,0.0,0.0 +0,1.364300,0.0,0.0,0.0,0.0,0.0 +0,1.364400,0.0,0.0,0.0,0.0,0.0 +0,1.364500,0.0,0.0,0.0,0.0,0.0 +0,1.364600,0.0,0.0,0.0,0.0,0.0 +0,1.364700,0.0,0.0,0.0,0.0,0.0 +0,1.364800,0.0,0.0,0.0,0.0,0.0 +0,1.364900,0.0,0.0,0.0,0.0,0.0 +0,1.365000,0.0,0.0,0.0,0.0,0.0 +0,1.365100,0.0,0.0,0.0,0.0,0.0 +0,1.365200,0.0,0.0,0.0,0.0,0.0 +0,1.365300,0.0,0.0,0.0,0.0,0.0 +0,1.365400,0.0,0.0,0.0,0.0,0.0 +0,1.365500,0.0,0.0,0.0,0.0,0.0 +0,1.365600,0.0,0.0,0.0,0.0,0.0 +0,1.365700,0.0,0.0,0.0,0.0,0.0 +0,1.365800,0.0,0.0,0.0,0.0,0.0 +0,1.365900,0.0,0.0,0.0,0.0,0.0 +0,1.366000,0.0,0.0,0.0,0.0,0.0 +0,1.366100,0.0,0.0,0.0,0.0,0.0 +0,1.366200,0.0,0.0,0.0,0.0,0.0 +0,1.366300,0.0,0.0,0.0,0.0,0.0 +0,1.366400,0.0,0.0,0.0,0.0,0.0 +0,1.366500,0.0,0.0,0.0,0.0,0.0 +0,1.366600,0.0,0.0,0.0,0.0,0.0 +0,1.366700,0.0,0.0,0.0,0.0,0.0 +0,1.366800,0.0,0.0,0.0,0.0,0.0 +0,1.366900,0.0,0.0,0.0,0.0,0.0 +0,1.367000,0.0,0.0,0.0,0.0,0.0 +0,1.367100,0.0,0.0,0.0,0.0,0.0 +0,1.367200,0.0,0.0,0.0,0.0,0.0 +0,1.367300,0.0,0.0,0.0,0.0,0.0 +0,1.367400,0.0,0.0,0.0,0.0,0.0 +0,1.367500,0.0,0.0,0.0,0.0,0.0 +0,1.367600,0.0,0.0,0.0,0.0,0.0 +0,1.367700,0.0,0.0,0.0,0.0,0.0 +0,1.367800,0.0,0.0,0.0,0.0,0.0 +0,1.367900,0.0,0.0,0.0,0.0,0.0 +0,1.368000,0.0,0.0,0.0,0.0,0.0 +0,1.368100,0.0,0.0,0.0,0.0,0.0 +0,1.368200,0.0,0.0,0.0,0.0,0.0 +0,1.368300,0.0,0.0,0.0,0.0,0.0 +0,1.368400,0.0,0.0,0.0,0.0,0.0 +0,1.368500,0.0,0.0,0.0,0.0,0.0 +0,1.368600,0.0,0.0,0.0,0.0,0.0 +0,1.368700,0.0,0.0,0.0,0.0,0.0 +0,1.368800,0.0,0.0,0.0,0.0,0.0 +0,1.368900,0.0,0.0,0.0,0.0,0.0 +0,1.369000,0.0,0.0,0.0,0.0,0.0 +0,1.369100,0.0,0.0,0.0,0.0,0.0 +0,1.369200,0.0,0.0,0.0,0.0,0.0 +0,1.369300,0.0,0.0,0.0,0.0,0.0 +0,1.369400,0.0,0.0,0.0,0.0,0.0 +0,1.369500,0.0,0.0,0.0,0.0,0.0 +0,1.369600,0.0,0.0,0.0,0.0,0.0 +0,1.369700,0.0,0.0,0.0,0.0,0.0 +0,1.369800,0.0,0.0,0.0,0.0,0.0 +0,1.369900,0.0,0.0,0.0,0.0,0.0 +0,1.370000,0.0,0.0,0.0,0.0,0.0 +0,1.370100,0.0,0.0,0.0,0.0,0.0 +0,1.370200,0.0,0.0,0.0,0.0,0.0 +0,1.370300,0.0,0.0,0.0,0.0,0.0 +0,1.370400,0.0,0.0,0.0,0.0,0.0 +0,1.370500,0.0,0.0,0.0,0.0,0.0 +0,1.370600,0.0,0.0,0.0,0.0,0.0 +0,1.370700,0.0,0.0,0.0,0.0,0.0 +0,1.370800,0.0,0.0,0.0,0.0,0.0 +0,1.370900,0.0,0.0,0.0,0.0,0.0 +0,1.371000,0.0,0.0,0.0,0.0,0.0 +0,1.371100,0.0,0.0,0.0,0.0,0.0 +0,1.371200,0.0,0.0,0.0,0.0,0.0 +0,1.371300,0.0,0.0,0.0,0.0,0.0 +0,1.371400,0.0,0.0,0.0,0.0,0.0 +0,1.371500,0.0,0.0,0.0,0.0,0.0 +0,1.371600,0.0,0.0,0.0,0.0,0.0 +0,1.371700,0.0,0.0,0.0,0.0,0.0 +0,1.371800,0.0,0.0,0.0,0.0,0.0 +0,1.371900,0.0,0.0,0.0,0.0,0.0 +0,1.372000,0.0,0.0,0.0,0.0,0.0 +0,1.372100,0.0,0.0,0.0,0.0,0.0 +0,1.372200,0.0,0.0,0.0,0.0,0.0 +0,1.372300,0.0,0.0,0.0,0.0,0.0 +0,1.372400,0.0,0.0,0.0,0.0,0.0 +0,1.372500,0.0,0.0,0.0,0.0,0.0 +0,1.372600,0.0,0.0,0.0,0.0,0.0 +0,1.372700,0.0,0.0,0.0,0.0,0.0 +0,1.372800,0.0,0.0,0.0,0.0,0.0 +0,1.372900,0.0,0.0,0.0,0.0,0.0 +0,1.373000,0.0,0.0,0.0,0.0,0.0 +0,1.373100,0.0,0.0,0.0,0.0,0.0 +0,1.373200,0.0,0.0,0.0,0.0,0.0 +0,1.373300,0.0,0.0,0.0,0.0,0.0 +0,1.373400,0.0,0.0,0.0,0.0,0.0 +0,1.373500,0.0,0.0,0.0,0.0,0.0 +0,1.373600,0.0,0.0,0.0,0.0,0.0 +0,1.373700,0.0,0.0,0.0,0.0,0.0 +0,1.373800,0.0,0.0,0.0,0.0,0.0 +0,1.373900,0.0,0.0,0.0,0.0,0.0 +0,1.374000,0.0,0.0,0.0,0.0,0.0 +0,1.374100,0.0,0.0,0.0,0.0,0.0 +0,1.374200,0.0,0.0,0.0,0.0,0.0 +0,1.374300,0.0,0.0,0.0,0.0,0.0 +0,1.374400,0.0,0.0,0.0,0.0,0.0 +0,1.374500,0.0,0.0,0.0,0.0,0.0 +0,1.374600,0.0,0.0,0.0,0.0,0.0 +0,1.374700,0.0,0.0,0.0,0.0,0.0 +0,1.374800,0.0,0.0,0.0,0.0,0.0 +0,1.374900,0.0,0.0,0.0,0.0,0.0 +0,1.375000,0.0,0.0,0.0,0.0,0.0 +0,1.375100,0.0,0.0,0.0,0.0,0.0 +0,1.375200,0.0,0.0,0.0,0.0,0.0 +0,1.375300,0.0,0.0,0.0,0.0,0.0 +0,1.375400,0.0,0.0,0.0,0.0,0.0 +0,1.375500,0.0,0.0,0.0,0.0,0.0 +0,1.375600,0.0,0.0,0.0,0.0,0.0 +0,1.375700,0.0,0.0,0.0,0.0,0.0 +0,1.375800,0.0,0.0,0.0,0.0,0.0 +0,1.375900,0.0,0.0,0.0,0.0,0.0 +0,1.376000,0.0,0.0,0.0,0.0,0.0 +0,1.376100,0.0,0.0,0.0,0.0,0.0 +0,1.376200,0.0,0.0,0.0,0.0,0.0 +0,1.376300,0.0,0.0,0.0,0.0,0.0 +0,1.376400,0.0,0.0,0.0,0.0,0.0 +0,1.376500,0.0,0.0,0.0,0.0,0.0 +0,1.376600,0.0,0.0,0.0,0.0,0.0 +0,1.376700,0.0,0.0,0.0,0.0,0.0 +0,1.376800,0.0,0.0,0.0,0.0,0.0 +0,1.376900,0.0,0.0,0.0,0.0,0.0 +0,1.377000,0.0,0.0,0.0,0.0,0.0 +0,1.377100,0.0,0.0,0.0,0.0,0.0 +0,1.377200,0.0,0.0,0.0,0.0,0.0 +0,1.377300,0.0,0.0,0.0,0.0,0.0 +0,1.377400,0.0,0.0,0.0,0.0,0.0 +0,1.377500,0.0,0.0,0.0,0.0,0.0 +0,1.377600,0.0,0.0,0.0,0.0,0.0 +0,1.377700,0.0,0.0,0.0,0.0,0.0 +0,1.377800,0.0,0.0,0.0,0.0,0.0 +0,1.377900,0.0,0.0,0.0,0.0,0.0 +0,1.378000,0.0,0.0,0.0,0.0,0.0 +0,1.378100,0.0,0.0,0.0,0.0,0.0 +0,1.378200,0.0,0.0,0.0,0.0,0.0 +0,1.378300,0.0,0.0,0.0,0.0,0.0 +0,1.378400,0.0,0.0,0.0,0.0,0.0 +0,1.378500,0.0,0.0,0.0,0.0,0.0 +0,1.378600,0.0,0.0,0.0,0.0,0.0 +0,1.378700,0.0,0.0,0.0,0.0,0.0 +0,1.378800,0.0,0.0,0.0,0.0,0.0 +0,1.378900,0.0,0.0,0.0,0.0,0.0 +0,1.379000,0.0,0.0,0.0,0.0,0.0 +0,1.379100,0.0,0.0,0.0,0.0,0.0 +0,1.379200,0.0,0.0,0.0,0.0,0.0 +0,1.379300,0.0,0.0,0.0,0.0,0.0 +0,1.379400,0.0,0.0,0.0,0.0,0.0 +0,1.379500,0.0,0.0,0.0,0.0,0.0 +0,1.379600,0.0,0.0,0.0,0.0,0.0 +0,1.379700,0.0,0.0,0.0,0.0,0.0 +0,1.379800,0.0,0.0,0.0,0.0,0.0 +0,1.379900,0.0,0.0,0.0,0.0,0.0 +0,1.380000,0.0,0.0,0.0,0.0,0.0 +0,1.380100,0.0,0.0,0.0,0.0,0.0 +1,1095.387112,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.380200,0.0,0.0,0.0,0.0,0.0 +0,1.380300,0.0,0.0,0.0,0.0,0.0 +0,1.380400,0.0,0.0,0.0,0.0,0.0 +0,1.380500,0.0,0.0,0.0,0.0,0.0 +0,1.380600,0.0,0.0,0.0,0.0,0.0 +0,1.380700,0.0,0.0,0.0,0.0,0.0 +0,1.380800,0.0,0.0,0.0,0.0,0.0 +0,1.380900,0.0,0.0,0.0,0.0,0.0 +0,1.381000,0.0,0.0,0.0,0.0,0.0 +0,1.381100,0.0,0.0,0.0,0.0,0.0 +0,1.381200,0.0,0.0,0.0,0.0,0.0 +0,1.381300,0.0,0.0,0.0,0.0,0.0 +0,1.381400,0.0,0.0,0.0,0.0,0.0 +0,1.381500,0.0,0.0,0.0,0.0,0.0 +0,1.381600,0.0,0.0,0.0,0.0,0.0 +0,1.381700,0.0,0.0,0.0,0.0,0.0 +0,1.381800,0.0,0.0,0.0,0.0,0.0 +0,1.381900,0.0,0.0,0.0,0.0,0.0 +0,1.382000,0.0,0.0,0.0,0.0,0.0 +0,1.382100,0.0,0.0,0.0,0.0,0.0 +0,1.382200,0.0,0.0,0.0,0.0,0.0 +0,1.382300,0.0,0.0,0.0,0.0,0.0 +0,1.382400,0.0,0.0,0.0,0.0,0.0 +0,1.382500,0.0,0.0,0.0,0.0,0.0 +0,1.382600,0.0,0.0,0.0,0.0,0.0 +0,1.382700,0.0,0.0,0.0,0.0,0.0 +0,1.382800,0.0,0.0,0.0,0.0,0.0 +0,1.382900,0.0,0.0,0.0,0.0,0.0 +0,1.383000,0.0,0.0,0.0,0.0,0.0 +0,1.383100,0.0,0.0,0.0,0.0,0.0 +0,1.383200,0.0,0.0,0.0,0.0,0.0 +0,1.383300,0.0,0.0,0.0,0.0,0.0 +0,1.383400,0.0,0.0,0.0,0.0,0.0 +0,1.383500,0.0,0.0,0.0,0.0,0.0 +0,1.383600,0.0,0.0,0.0,0.0,0.0 +0,1.383700,0.0,0.0,0.0,0.0,0.0 +0,1.383800,0.0,0.0,0.0,0.0,0.0 +0,1.383900,0.0,0.0,0.0,0.0,0.0 +0,1.384000,0.0,0.0,0.0,0.0,0.0 +0,1.384100,0.0,0.0,0.0,0.0,0.0 +0,1.384200,0.0,0.0,0.0,0.0,0.0 +0,1.384300,0.0,0.0,0.0,0.0,0.0 +0,1.384400,0.0,0.0,0.0,0.0,0.0 +0,1.384500,0.0,0.0,0.0,0.0,0.0 +0,1.384600,0.0,0.0,0.0,0.0,0.0 +0,1.384700,0.0,0.0,0.0,0.0,0.0 +0,1.384800,0.0,0.0,0.0,0.0,0.0 +0,1.384900,0.0,0.0,0.0,0.0,0.0 +0,1.385000,0.0,0.0,0.0,0.0,0.0 +0,1.385100,0.0,0.0,0.0,0.0,0.0 +0,1.385200,0.0,0.0,0.0,0.0,0.0 +0,1.385300,0.0,0.0,0.0,0.0,0.0 +0,1.385400,0.0,0.0,0.0,0.0,0.0 +0,1.385500,0.0,0.0,0.0,0.0,0.0 +0,1.385600,0.0,0.0,0.0,0.0,0.0 +0,1.385700,0.0,0.0,0.0,0.0,0.0 +0,1.385800,0.0,0.0,0.0,0.0,0.0 +0,1.385900,0.0,0.0,0.0,0.0,0.0 +0,1.386000,0.0,0.0,0.0,0.0,0.0 +0,1.386100,0.0,0.0,0.0,0.0,0.0 +0,1.386200,0.0,0.0,0.0,0.0,0.0 +0,1.386300,0.0,0.0,0.0,0.0,0.0 +0,1.386400,0.0,0.0,0.0,0.0,0.0 +0,1.386500,0.0,0.0,0.0,0.0,0.0 +0,1.386600,0.0,0.0,0.0,0.0,0.0 +0,1.386700,0.0,0.0,0.0,0.0,0.0 +0,1.386800,0.0,0.0,0.0,0.0,0.0 +0,1.386900,0.0,0.0,0.0,0.0,0.0 +0,1.387000,0.0,0.0,0.0,0.0,0.0 +0,1.387100,0.0,0.0,0.0,0.0,0.0 +0,1.387200,0.0,0.0,0.0,0.0,0.0 +0,1.387300,0.0,0.0,0.0,0.0,0.0 +0,1.387400,0.0,0.0,0.0,0.0,0.0 +0,1.387500,0.0,0.0,0.0,0.0,0.0 +0,1.387600,0.0,0.0,0.0,0.0,0.0 +0,1.387700,0.0,0.0,0.0,0.0,0.0 +0,1.387800,0.0,0.0,0.0,0.0,0.0 +0,1.387900,0.0,0.0,0.0,0.0,0.0 +0,1.388000,0.0,0.0,0.0,0.0,0.0 +0,1.388100,0.0,0.0,0.0,0.0,0.0 +0,1.388200,0.0,0.0,0.0,0.0,0.0 +0,1.388300,0.0,0.0,0.0,0.0,0.0 +0,1.388400,0.0,0.0,0.0,0.0,0.0 +0,1.388500,0.0,0.0,0.0,0.0,0.0 +0,1.388600,0.0,0.0,0.0,0.0,0.0 +0,1.388700,0.0,0.0,0.0,0.0,0.0 +0,1.388800,0.0,0.0,0.0,0.0,0.0 +0,1.388900,0.0,0.0,0.0,0.0,0.0 +0,1.389000,0.0,0.0,0.0,0.0,0.0 +0,1.389100,0.0,0.0,0.0,0.0,0.0 +0,1.389200,0.0,0.0,0.0,0.0,0.0 +0,1.389300,0.0,0.0,0.0,0.0,0.0 +0,1.389400,0.0,0.0,0.0,0.0,0.0 +0,1.389500,0.0,0.0,0.0,0.0,0.0 +0,1.389600,0.0,0.0,0.0,0.0,0.0 +0,1.389700,0.0,0.0,0.0,0.0,0.0 +0,1.389800,0.0,0.0,0.0,0.0,0.0 +0,1.389900,0.0,0.0,0.0,0.0,0.0 +0,1.390000,0.0,0.0,0.0,0.0,0.0 +0,1.390100,0.0,0.0,0.0,0.0,0.0 +0,1.390200,0.0,0.0,0.0,0.0,0.0 +0,1.390300,0.0,0.0,0.0,0.0,0.0 +0,1.390400,0.0,0.0,0.0,0.0,0.0 +0,1.390500,0.0,0.0,0.0,0.0,0.0 +0,1.390600,0.0,0.0,0.0,0.0,0.0 +0,1.390700,0.0,0.0,0.0,0.0,0.0 +0,1.390800,0.0,0.0,0.0,0.0,0.0 +0,1.390900,0.0,0.0,0.0,0.0,0.0 +0,1.391000,0.0,0.0,0.0,0.0,0.0 +0,1.391100,0.0,0.0,0.0,0.0,0.0 +0,1.391200,0.0,0.0,0.0,0.0,0.0 +0,1.391300,0.0,0.0,0.0,0.0,0.0 +0,1.391400,0.0,0.0,0.0,0.0,0.0 +0,1.391500,0.0,0.0,0.0,0.0,0.0 +0,1.391600,0.0,0.0,0.0,0.0,0.0 +0,1.391700,0.0,0.0,0.0,0.0,0.0 +0,1.391800,0.0,0.0,0.0,0.0,0.0 +0,1.391900,0.0,0.0,0.0,0.0,0.0 +0,1.392000,0.0,0.0,0.0,0.0,0.0 +0,1.392100,0.0,0.0,0.0,0.0,0.0 +0,1.392200,0.0,0.0,0.0,0.0,0.0 +0,1.392300,0.0,0.0,0.0,0.0,0.0 +0,1.392400,0.0,0.0,0.0,0.0,0.0 +0,1.392500,0.0,0.0,0.0,0.0,0.0 +0,1.392600,0.0,0.0,0.0,0.0,0.0 +0,1.392700,0.0,0.0,0.0,0.0,0.0 +0,1.392800,0.0,0.0,0.0,0.0,0.0 +0,1.392900,0.0,0.0,0.0,0.0,0.0 +0,1.393000,0.0,0.0,0.0,0.0,0.0 +0,1.393100,0.0,0.0,0.0,0.0,0.0 +0,1.393200,0.0,0.0,0.0,0.0,0.0 +0,1.393300,0.0,0.0,0.0,0.0,0.0 +0,1.393400,0.0,0.0,0.0,0.0,0.0 +0,1.393500,0.0,0.0,0.0,0.0,0.0 +0,1.393600,0.0,0.0,0.0,0.0,0.0 +0,1.393700,0.0,0.0,0.0,0.0,0.0 +0,1.393800,0.0,0.0,0.0,0.0,0.0 +0,1.393900,0.0,0.0,0.0,0.0,0.0 +0,1.394000,0.0,0.0,0.0,0.0,0.0 +0,1.394100,0.0,0.0,0.0,0.0,0.0 +0,1.394200,0.0,0.0,0.0,0.0,0.0 +0,1.394300,0.0,0.0,0.0,0.0,0.0 +0,1.394400,0.0,0.0,0.0,0.0,0.0 +0,1.394500,0.0,0.0,0.0,0.0,0.0 +0,1.394600,0.0,0.0,0.0,0.0,0.0 +0,1.394700,0.0,0.0,0.0,0.0,0.0 +0,1.394800,0.0,0.0,0.0,0.0,0.0 +0,1.394900,0.0,0.0,0.0,0.0,0.0 +0,1.395000,0.0,0.0,0.0,0.0,0.0 +0,1.395100,0.0,0.0,0.0,0.0,0.0 +0,1.395200,0.0,0.0,0.0,0.0,0.0 +0,1.395300,0.0,0.0,0.0,0.0,0.0 +0,1.395400,0.0,0.0,0.0,0.0,0.0 +0,1.395500,0.0,0.0,0.0,0.0,0.0 +0,1.395600,0.0,0.0,0.0,0.0,0.0 +0,1.395700,0.0,0.0,0.0,0.0,0.0 +0,1.395800,0.0,0.0,0.0,0.0,0.0 +0,1.395900,0.0,0.0,0.0,0.0,0.0 +0,1.396000,0.0,0.0,0.0,0.0,0.0 +0,1.396100,0.0,0.0,0.0,0.0,0.0 +0,1.396200,0.0,0.0,0.0,0.0,0.0 +0,1.396300,0.0,0.0,0.0,0.0,0.0 +0,1.396400,0.0,0.0,0.0,0.0,0.0 +0,1.396500,0.0,0.0,0.0,0.0,0.0 +0,1.396600,0.0,0.0,0.0,0.0,0.0 +0,1.396700,0.0,0.0,0.0,0.0,0.0 +0,1.396800,0.0,0.0,0.0,0.0,0.0 +0,1.396900,0.0,0.0,0.0,0.0,0.0 +0,1.397000,0.0,0.0,0.0,0.0,0.0 +0,1.397100,0.0,0.0,0.0,0.0,0.0 +0,1.397200,0.0,0.0,0.0,0.0,0.0 +0,1.397300,0.0,0.0,0.0,0.0,0.0 +0,1.397400,0.0,0.0,0.0,0.0,0.0 +0,1.397500,0.0,0.0,0.0,0.0,0.0 +0,1.397600,0.0,0.0,0.0,0.0,0.0 +0,1.397700,0.0,0.0,0.0,0.0,0.0 +0,1.397800,0.0,0.0,0.0,0.0,0.0 +0,1.397900,0.0,0.0,0.0,0.0,0.0 +0,1.398000,0.0,0.0,0.0,0.0,0.0 +0,1.398100,0.0,0.0,0.0,0.0,0.0 +0,1.398200,0.0,0.0,0.0,0.0,0.0 +0,1.398300,0.0,0.0,0.0,0.0,0.0 +0,1.398400,0.0,0.0,0.0,0.0,0.0 +0,1.398500,0.0,0.0,0.0,0.0,0.0 +0,1.398600,0.0,0.0,0.0,0.0,0.0 +0,1.398700,0.0,0.0,0.0,0.0,0.0 +0,1.398800,0.0,0.0,0.0,0.0,0.0 +0,1.398900,0.0,0.0,0.0,0.0,0.0 +0,1.399000,0.0,0.0,0.0,0.0,0.0 +0,1.399100,0.0,0.0,0.0,0.0,0.0 +0,1.399200,0.0,0.0,0.0,0.0,0.0 +0,1.399300,0.0,0.0,0.0,0.0,0.0 +0,1.399400,0.0,0.0,0.0,0.0,0.0 +0,1.399500,0.0,0.0,0.0,0.0,0.0 +0,1.399600,0.0,0.0,0.0,0.0,0.0 +0,1.399700,0.0,0.0,0.0,0.0,0.0 +0,1.399800,0.0,0.0,0.0,0.0,0.0 +0,1.399900,0.0,0.0,0.0,0.0,0.0 +0,1.400000,0.0,0.0,0.0,0.0,0.0 +0,1.400100,0.0,0.0,0.0,0.0,0.0 +1,1143.700871,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.400200,0.0,0.0,0.0,0.0,0.0 +0,1.400300,0.0,0.0,0.0,0.0,0.0 +0,1.400400,0.0,0.0,0.0,0.0,0.0 +0,1.400500,0.0,0.0,0.0,0.0,0.0 +0,1.400600,0.0,0.0,0.0,0.0,0.0 +0,1.400700,0.0,0.0,0.0,0.0,0.0 +0,1.400800,0.0,0.0,0.0,0.0,0.0 +0,1.400900,0.0,0.0,0.0,0.0,0.0 +0,1.401000,0.0,0.0,0.0,0.0,0.0 +0,1.401100,0.0,0.0,0.0,0.0,0.0 +0,1.401200,0.0,0.0,0.0,0.0,0.0 +0,1.401300,0.0,0.0,0.0,0.0,0.0 +0,1.401400,0.0,0.0,0.0,0.0,0.0 +0,1.401500,0.0,0.0,0.0,0.0,0.0 +0,1.401600,0.0,0.0,0.0,0.0,0.0 +0,1.401700,0.0,0.0,0.0,0.0,0.0 +0,1.401800,0.0,0.0,0.0,0.0,0.0 +0,1.401900,0.0,0.0,0.0,0.0,0.0 +0,1.402000,0.0,0.0,0.0,0.0,0.0 +0,1.402100,0.0,0.0,0.0,0.0,0.0 +0,1.402200,0.0,0.0,0.0,0.0,0.0 +0,1.402300,0.0,0.0,0.0,0.0,0.0 +0,1.402400,0.0,0.0,0.0,0.0,0.0 +0,1.402500,0.0,0.0,0.0,0.0,0.0 +0,1.402600,0.0,0.0,0.0,0.0,0.0 +0,1.402700,0.0,0.0,0.0,0.0,0.0 +0,1.402800,0.0,0.0,0.0,0.0,0.0 +0,1.402900,0.0,0.0,0.0,0.0,0.0 +0,1.403000,0.0,0.0,0.0,0.0,0.0 +0,1.403100,0.0,0.0,0.0,0.0,0.0 +0,1.403200,0.0,0.0,0.0,0.0,0.0 +0,1.403300,0.0,0.0,0.0,0.0,0.0 +0,1.403400,0.0,0.0,0.0,0.0,0.0 +0,1.403500,0.0,0.0,0.0,0.0,0.0 +0,1.403600,0.0,0.0,0.0,0.0,0.0 +0,1.403700,0.0,0.0,0.0,0.0,0.0 +0,1.403800,0.0,0.0,0.0,0.0,0.0 +0,1.403900,0.0,0.0,0.0,0.0,0.0 +0,1.404000,0.0,0.0,0.0,0.0,0.0 +0,1.404100,0.0,0.0,0.0,0.0,0.0 +0,1.404200,0.0,0.0,0.0,0.0,0.0 +0,1.404300,0.0,0.0,0.0,0.0,0.0 +0,1.404400,0.0,0.0,0.0,0.0,0.0 +0,1.404500,0.0,0.0,0.0,0.0,0.0 +0,1.404600,0.0,0.0,0.0,0.0,0.0 +0,1.404700,0.0,0.0,0.0,0.0,0.0 +0,1.404800,0.0,0.0,0.0,0.0,0.0 +0,1.404900,0.0,0.0,0.0,0.0,0.0 +0,1.405000,0.0,0.0,0.0,0.0,0.0 +0,1.405100,0.0,0.0,0.0,0.0,0.0 +0,1.405200,0.0,0.0,0.0,0.0,0.0 +0,1.405300,0.0,0.0,0.0,0.0,0.0 +0,1.405400,0.0,0.0,0.0,0.0,0.0 +0,1.405500,0.0,0.0,0.0,0.0,0.0 +0,1.405600,0.0,0.0,0.0,0.0,0.0 +0,1.405700,0.0,0.0,0.0,0.0,0.0 +0,1.405800,0.0,0.0,0.0,0.0,0.0 +0,1.405900,0.0,0.0,0.0,0.0,0.0 +0,1.406000,0.0,0.0,0.0,0.0,0.0 +0,1.406100,0.0,0.0,0.0,0.0,0.0 +0,1.406200,0.0,0.0,0.0,0.0,0.0 +0,1.406300,0.0,0.0,0.0,0.0,0.0 +0,1.406400,0.0,0.0,0.0,0.0,0.0 +0,1.406500,0.0,0.0,0.0,0.0,0.0 +0,1.406600,0.0,0.0,0.0,0.0,0.0 +0,1.406700,0.0,0.0,0.0,0.0,0.0 +0,1.406800,0.0,0.0,0.0,0.0,0.0 +0,1.406900,0.0,0.0,0.0,0.0,0.0 +0,1.407000,0.0,0.0,0.0,0.0,0.0 +0,1.407100,0.0,0.0,0.0,0.0,0.0 +0,1.407200,0.0,0.0,0.0,0.0,0.0 +0,1.407300,0.0,0.0,0.0,0.0,0.0 +0,1.407400,0.0,0.0,0.0,0.0,0.0 +0,1.407500,0.0,0.0,0.0,0.0,0.0 +0,1.407600,0.0,0.0,0.0,0.0,0.0 +0,1.407700,0.0,0.0,0.0,0.0,0.0 +0,1.407800,0.0,0.0,0.0,0.0,0.0 +0,1.407900,0.0,0.0,0.0,0.0,0.0 +0,1.408000,0.0,0.0,0.0,0.0,0.0 +0,1.408100,0.0,0.0,0.0,0.0,0.0 +0,1.408200,0.0,0.0,0.0,0.0,0.0 +0,1.408300,0.0,0.0,0.0,0.0,0.0 +0,1.408400,0.0,0.0,0.0,0.0,0.0 +0,1.408500,0.0,0.0,0.0,0.0,0.0 +0,1.408600,0.0,0.0,0.0,0.0,0.0 +0,1.408700,0.0,0.0,0.0,0.0,0.0 +0,1.408800,0.0,0.0,0.0,0.0,0.0 +0,1.408900,0.0,0.0,0.0,0.0,0.0 +0,1.409000,0.0,0.0,0.0,0.0,0.0 +0,1.409100,0.0,0.0,0.0,0.0,0.0 +0,1.409200,0.0,0.0,0.0,0.0,0.0 +0,1.409300,0.0,0.0,0.0,0.0,0.0 +0,1.409400,0.0,0.0,0.0,0.0,0.0 +0,1.409500,0.0,0.0,0.0,0.0,0.0 +0,1.409600,0.0,0.0,0.0,0.0,0.0 +0,1.409700,0.0,0.0,0.0,0.0,0.0 +0,1.409800,0.0,0.0,0.0,0.0,0.0 +0,1.409900,0.0,0.0,0.0,0.0,0.0 +0,1.410000,0.0,0.0,0.0,0.0,0.0 +0,1.410100,0.0,0.0,0.0,0.0,0.0 +0,1.410200,0.0,0.0,0.0,0.0,0.0 +0,1.410300,0.0,0.0,0.0,0.0,0.0 +0,1.410400,0.0,0.0,0.0,0.0,0.0 +0,1.410500,0.0,0.0,0.0,0.0,0.0 +0,1.410600,0.0,0.0,0.0,0.0,0.0 +0,1.410700,0.0,0.0,0.0,0.0,0.0 +0,1.410800,0.0,0.0,0.0,0.0,0.0 +0,1.410900,0.0,0.0,0.0,0.0,0.0 +0,1.411000,0.0,0.0,0.0,0.0,0.0 +0,1.411100,0.0,0.0,0.0,0.0,0.0 +0,1.411200,0.0,0.0,0.0,0.0,0.0 +0,1.411300,0.0,0.0,0.0,0.0,0.0 +0,1.411400,0.0,0.0,0.0,0.0,0.0 +0,1.411500,0.0,0.0,0.0,0.0,0.0 +0,1.411600,0.0,0.0,0.0,0.0,0.0 +0,1.411700,0.0,0.0,0.0,0.0,0.0 +0,1.411800,0.0,0.0,0.0,0.0,0.0 +0,1.411900,0.0,0.0,0.0,0.0,0.0 +0,1.412000,0.0,0.0,0.0,0.0,0.0 +0,1.412100,0.0,0.0,0.0,0.0,0.0 +0,1.412200,0.0,0.0,0.0,0.0,0.0 +0,1.412300,0.0,0.0,0.0,0.0,0.0 +0,1.412400,0.0,0.0,0.0,0.0,0.0 +0,1.412500,0.0,0.0,0.0,0.0,0.0 +0,1.412600,0.0,0.0,0.0,0.0,0.0 +0,1.412700,0.0,0.0,0.0,0.0,0.0 +0,1.412800,0.0,0.0,0.0,0.0,0.0 +0,1.412900,0.0,0.0,0.0,0.0,0.0 +0,1.413000,0.0,0.0,0.0,0.0,0.0 +0,1.413100,0.0,0.0,0.0,0.0,0.0 +0,1.413200,0.0,0.0,0.0,0.0,0.0 +0,1.413300,0.0,0.0,0.0,0.0,0.0 +0,1.413400,0.0,0.0,0.0,0.0,0.0 +0,1.413500,0.0,0.0,0.0,0.0,0.0 +0,1.413600,0.0,0.0,0.0,0.0,0.0 +0,1.413700,0.0,0.0,0.0,0.0,0.0 +0,1.413800,0.0,0.0,0.0,0.0,0.0 +0,1.413900,0.0,0.0,0.0,0.0,0.0 +0,1.414000,0.0,0.0,0.0,0.0,0.0 +0,1.414100,0.0,0.0,0.0,0.0,0.0 +0,1.414200,0.0,0.0,0.0,0.0,0.0 +0,1.414300,0.0,0.0,0.0,0.0,0.0 +0,1.414400,0.0,0.0,0.0,0.0,0.0 +0,1.414500,0.0,0.0,0.0,0.0,0.0 +0,1.414600,0.0,0.0,0.0,0.0,0.0 +0,1.414700,0.0,0.0,0.0,0.0,0.0 +0,1.414800,0.0,0.0,0.0,0.0,0.0 +0,1.414900,0.0,0.0,0.0,0.0,0.0 +0,1.415000,0.0,0.0,0.0,0.0,0.0 +0,1.415100,0.0,0.0,0.0,0.0,0.0 +0,1.415200,0.0,0.0,0.0,0.0,0.0 +0,1.415300,0.0,0.0,0.0,0.0,0.0 +0,1.415400,0.0,0.0,0.0,0.0,0.0 +0,1.415500,0.0,0.0,0.0,0.0,0.0 +0,1.415600,0.0,0.0,0.0,0.0,0.0 +0,1.415700,0.0,0.0,0.0,0.0,0.0 +0,1.415800,0.0,0.0,0.0,0.0,0.0 +0,1.415900,0.0,0.0,0.0,0.0,0.0 +0,1.416000,0.0,0.0,0.0,0.0,0.0 +0,1.416100,0.0,0.0,0.0,0.0,0.0 +0,1.416200,0.0,0.0,0.0,0.0,0.0 +0,1.416300,0.0,0.0,0.0,0.0,0.0 +0,1.416400,0.0,0.0,0.0,0.0,0.0 +0,1.416500,0.0,0.0,0.0,0.0,0.0 +0,1.416600,0.0,0.0,0.0,0.0,0.0 +0,1.416700,0.0,0.0,0.0,0.0,0.0 +0,1.416800,0.0,0.0,0.0,0.0,0.0 +0,1.416900,0.0,0.0,0.0,0.0,0.0 +0,1.417000,0.0,0.0,0.0,0.0,0.0 +0,1.417100,0.0,0.0,0.0,0.0,0.0 +0,1.417200,0.0,0.0,0.0,0.0,0.0 +0,1.417300,0.0,0.0,0.0,0.0,0.0 +0,1.417400,0.0,0.0,0.0,0.0,0.0 +0,1.417500,0.0,0.0,0.0,0.0,0.0 +0,1.417600,0.0,0.0,0.0,0.0,0.0 +0,1.417700,0.0,0.0,0.0,0.0,0.0 +0,1.417800,0.0,0.0,0.0,0.0,0.0 +0,1.417900,0.0,0.0,0.0,0.0,0.0 +0,1.418000,0.0,0.0,0.0,0.0,0.0 +0,1.418100,0.0,0.0,0.0,0.0,0.0 +0,1.418200,0.0,0.0,0.0,0.0,0.0 +0,1.418300,0.0,0.0,0.0,0.0,0.0 +0,1.418400,0.0,0.0,0.0,0.0,0.0 +0,1.418500,0.0,0.0,0.0,0.0,0.0 +0,1.418600,0.0,0.0,0.0,0.0,0.0 +0,1.418700,0.0,0.0,0.0,0.0,0.0 +0,1.418800,0.0,0.0,0.0,0.0,0.0 +0,1.418900,0.0,0.0,0.0,0.0,0.0 +0,1.419000,0.0,0.0,0.0,0.0,0.0 +0,1.419100,0.0,0.0,0.0,0.0,0.0 +0,1.419200,0.0,0.0,0.0,0.0,0.0 +0,1.419300,0.0,0.0,0.0,0.0,0.0 +0,1.419400,0.0,0.0,0.0,0.0,0.0 +0,1.419500,0.0,0.0,0.0,0.0,0.0 +0,1.419600,0.0,0.0,0.0,0.0,0.0 +0,1.419700,0.0,0.0,0.0,0.0,0.0 +0,1.419800,0.0,0.0,0.0,0.0,0.0 +0,1.419900,0.0,0.0,0.0,0.0,0.0 +0,1.420000,0.0,0.0,0.0,0.0,0.0 +0,1.420100,0.0,0.0,0.0,0.0,0.0 +1,1193.414780,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.420200,0.0,0.0,0.0,0.0,0.0 +0,1.420300,0.0,0.0,0.0,0.0,0.0 +0,1.420400,0.0,0.0,0.0,0.0,0.0 +0,1.420500,0.0,0.0,0.0,0.0,0.0 +0,1.420600,0.0,0.0,0.0,0.0,0.0 +0,1.420700,0.0,0.0,0.0,0.0,0.0 +0,1.420800,0.0,0.0,0.0,0.0,0.0 +0,1.420900,0.0,0.0,0.0,0.0,0.0 +0,1.421000,0.0,0.0,0.0,0.0,0.0 +0,1.421100,0.0,0.0,0.0,0.0,0.0 +0,1.421200,0.0,0.0,0.0,0.0,0.0 +0,1.421300,0.0,0.0,0.0,0.0,0.0 +0,1.421400,0.0,0.0,0.0,0.0,0.0 +0,1.421500,0.0,0.0,0.0,0.0,0.0 +0,1.421600,0.0,0.0,0.0,0.0,0.0 +0,1.421700,0.0,0.0,0.0,0.0,0.0 +0,1.421800,0.0,0.0,0.0,0.0,0.0 +0,1.421900,0.0,0.0,0.0,0.0,0.0 +0,1.422000,0.0,0.0,0.0,0.0,0.0 +0,1.422100,0.0,0.0,0.0,0.0,0.0 +0,1.422200,0.0,0.0,0.0,0.0,0.0 +0,1.422300,0.0,0.0,0.0,0.0,0.0 +0,1.422400,0.0,0.0,0.0,0.0,0.0 +0,1.422500,0.0,0.0,0.0,0.0,0.0 +0,1.422600,0.0,0.0,0.0,0.0,0.0 +0,1.422700,0.0,0.0,0.0,0.0,0.0 +0,1.422800,0.0,0.0,0.0,0.0,0.0 +0,1.422900,0.0,0.0,0.0,0.0,0.0 +0,1.423000,0.0,0.0,0.0,0.0,0.0 +0,1.423100,0.0,0.0,0.0,0.0,0.0 +0,1.423200,0.0,0.0,0.0,0.0,0.0 +0,1.423300,0.0,0.0,0.0,0.0,0.0 +0,1.423400,0.0,0.0,0.0,0.0,0.0 +0,1.423500,0.0,0.0,0.0,0.0,0.0 +0,1.423600,0.0,0.0,0.0,0.0,0.0 +0,1.423700,0.0,0.0,0.0,0.0,0.0 +0,1.423800,0.0,0.0,0.0,0.0,0.0 +0,1.423900,0.0,0.0,0.0,0.0,0.0 +0,1.424000,0.0,0.0,0.0,0.0,0.0 +0,1.424100,0.0,0.0,0.0,0.0,0.0 +0,1.424200,0.0,0.0,0.0,0.0,0.0 +0,1.424300,0.0,0.0,0.0,0.0,0.0 +0,1.424400,0.0,0.0,0.0,0.0,0.0 +0,1.424500,0.0,0.0,0.0,0.0,0.0 +0,1.424600,0.0,0.0,0.0,0.0,0.0 +0,1.424700,0.0,0.0,0.0,0.0,0.0 +0,1.424800,0.0,0.0,0.0,0.0,0.0 +0,1.424900,0.0,0.0,0.0,0.0,0.0 +0,1.425000,0.0,0.0,0.0,0.0,0.0 +0,1.425100,0.0,0.0,0.0,0.0,0.0 +0,1.425200,0.0,0.0,0.0,0.0,0.0 +0,1.425300,0.0,0.0,0.0,0.0,0.0 +0,1.425400,0.0,0.0,0.0,0.0,0.0 +0,1.425500,0.0,0.0,0.0,0.0,0.0 +0,1.425600,0.0,0.0,0.0,0.0,0.0 +0,1.425700,0.0,0.0,0.0,0.0,0.0 +0,1.425800,0.0,0.0,0.0,0.0,0.0 +0,1.425900,0.0,0.0,0.0,0.0,0.0 +0,1.426000,0.0,0.0,0.0,0.0,0.0 +0,1.426100,0.0,0.0,0.0,0.0,0.0 +0,1.426200,0.0,0.0,0.0,0.0,0.0 +0,1.426300,0.0,0.0,0.0,0.0,0.0 +0,1.426400,0.0,0.0,0.0,0.0,0.0 +0,1.426500,0.0,0.0,0.0,0.0,0.0 +0,1.426600,0.0,0.0,0.0,0.0,0.0 +0,1.426700,0.0,0.0,0.0,0.0,0.0 +0,1.426800,0.0,0.0,0.0,0.0,0.0 +0,1.426900,0.0,0.0,0.0,0.0,0.0 +0,1.427000,0.0,0.0,0.0,0.0,0.0 +0,1.427100,0.0,0.0,0.0,0.0,0.0 +0,1.427200,0.0,0.0,0.0,0.0,0.0 +0,1.427300,0.0,0.0,0.0,0.0,0.0 +0,1.427400,0.0,0.0,0.0,0.0,0.0 +0,1.427500,0.0,0.0,0.0,0.0,0.0 +0,1.427600,0.0,0.0,0.0,0.0,0.0 +0,1.427700,0.0,0.0,0.0,0.0,0.0 +0,1.427800,0.0,0.0,0.0,0.0,0.0 +0,1.427900,0.0,0.0,0.0,0.0,0.0 +0,1.428000,0.0,0.0,0.0,0.0,0.0 +0,1.428100,0.0,0.0,0.0,0.0,0.0 +0,1.428200,0.0,0.0,0.0,0.0,0.0 +0,1.428300,0.0,0.0,0.0,0.0,0.0 +0,1.428400,0.0,0.0,0.0,0.0,0.0 +0,1.428500,0.0,0.0,0.0,0.0,0.0 +0,1.428600,0.0,0.0,0.0,0.0,0.0 +0,1.428700,0.0,0.0,0.0,0.0,0.0 +0,1.428800,0.0,0.0,0.0,0.0,0.0 +0,1.428900,0.0,0.0,0.0,0.0,0.0 +0,1.429000,0.0,0.0,0.0,0.0,0.0 +0,1.429100,0.0,0.0,0.0,0.0,0.0 +0,1.429200,0.0,0.0,0.0,0.0,0.0 +0,1.429300,0.0,0.0,0.0,0.0,0.0 +0,1.429400,0.0,0.0,0.0,0.0,0.0 +0,1.429500,0.0,0.0,0.0,0.0,0.0 +0,1.429600,0.0,0.0,0.0,0.0,0.0 +0,1.429700,0.0,0.0,0.0,0.0,0.0 +0,1.429800,0.0,0.0,0.0,0.0,0.0 +0,1.429900,0.0,0.0,0.0,0.0,0.0 +0,1.430000,0.0,0.0,0.0,0.0,0.0 +0,1.430100,0.0,0.0,0.0,0.0,0.0 +0,1.430200,0.0,0.0,0.0,0.0,0.0 +0,1.430300,0.0,0.0,0.0,0.0,0.0 +0,1.430400,0.0,0.0,0.0,0.0,0.0 +0,1.430500,0.0,0.0,0.0,0.0,0.0 +0,1.430600,0.0,0.0,0.0,0.0,0.0 +0,1.430700,0.0,0.0,0.0,0.0,0.0 +0,1.430800,0.0,0.0,0.0,0.0,0.0 +0,1.430900,0.0,0.0,0.0,0.0,0.0 +0,1.431000,0.0,0.0,0.0,0.0,0.0 +0,1.431100,0.0,0.0,0.0,0.0,0.0 +0,1.431200,0.0,0.0,0.0,0.0,0.0 +0,1.431300,0.0,0.0,0.0,0.0,0.0 +0,1.431400,0.0,0.0,0.0,0.0,0.0 +0,1.431500,0.0,0.0,0.0,0.0,0.0 +0,1.431600,0.0,0.0,0.0,0.0,0.0 +0,1.431700,0.0,0.0,0.0,0.0,0.0 +0,1.431800,0.0,0.0,0.0,0.0,0.0 +0,1.431900,0.0,0.0,0.0,0.0,0.0 +0,1.432000,0.0,0.0,0.0,0.0,0.0 +0,1.432100,0.0,0.0,0.0,0.0,0.0 +0,1.432200,0.0,0.0,0.0,0.0,0.0 +0,1.432300,0.0,0.0,0.0,0.0,0.0 +0,1.432400,0.0,0.0,0.0,0.0,0.0 +0,1.432500,0.0,0.0,0.0,0.0,0.0 +0,1.432600,0.0,0.0,0.0,0.0,0.0 +0,1.432700,0.0,0.0,0.0,0.0,0.0 +0,1.432800,0.0,0.0,0.0,0.0,0.0 +0,1.432900,0.0,0.0,0.0,0.0,0.0 +0,1.433000,0.0,0.0,0.0,0.0,0.0 +0,1.433100,0.0,0.0,0.0,0.0,0.0 +0,1.433200,0.0,0.0,0.0,0.0,0.0 +0,1.433300,0.0,0.0,0.0,0.0,0.0 +0,1.433400,0.0,0.0,0.0,0.0,0.0 +0,1.433500,0.0,0.0,0.0,0.0,0.0 +0,1.433600,0.0,0.0,0.0,0.0,0.0 +0,1.433700,0.0,0.0,0.0,0.0,0.0 +0,1.433800,0.0,0.0,0.0,0.0,0.0 +0,1.433900,0.0,0.0,0.0,0.0,0.0 +0,1.434000,0.0,0.0,0.0,0.0,0.0 +0,1.434100,0.0,0.0,0.0,0.0,0.0 +0,1.434200,0.0,0.0,0.0,0.0,0.0 +0,1.434300,0.0,0.0,0.0,0.0,0.0 +0,1.434400,0.0,0.0,0.0,0.0,0.0 +0,1.434500,0.0,0.0,0.0,0.0,0.0 +0,1.434600,0.0,0.0,0.0,0.0,0.0 +0,1.434700,0.0,0.0,0.0,0.0,0.0 +0,1.434800,0.0,0.0,0.0,0.0,0.0 +0,1.434900,0.0,0.0,0.0,0.0,0.0 +0,1.435000,0.0,0.0,0.0,0.0,0.0 +0,1.435100,0.0,0.0,0.0,0.0,0.0 +0,1.435200,0.0,0.0,0.0,0.0,0.0 +0,1.435300,0.0,0.0,0.0,0.0,0.0 +0,1.435400,0.0,0.0,0.0,0.0,0.0 +0,1.435500,0.0,0.0,0.0,0.0,0.0 +0,1.435600,0.0,0.0,0.0,0.0,0.0 +0,1.435700,0.0,0.0,0.0,0.0,0.0 +0,1.435800,0.0,0.0,0.0,0.0,0.0 +0,1.435900,0.0,0.0,0.0,0.0,0.0 +0,1.436000,0.0,0.0,0.0,0.0,0.0 +0,1.436100,0.0,0.0,0.0,0.0,0.0 +0,1.436200,0.0,0.0,0.0,0.0,0.0 +0,1.436300,0.0,0.0,0.0,0.0,0.0 +0,1.436400,0.0,0.0,0.0,0.0,0.0 +0,1.436500,0.0,0.0,0.0,0.0,0.0 +0,1.436600,0.0,0.0,0.0,0.0,0.0 +0,1.436700,0.0,0.0,0.0,0.0,0.0 +0,1.436800,0.0,0.0,0.0,0.0,0.0 +0,1.436900,0.0,0.0,0.0,0.0,0.0 +0,1.437000,0.0,0.0,0.0,0.0,0.0 +0,1.437100,0.0,0.0,0.0,0.0,0.0 +0,1.437200,0.0,0.0,0.0,0.0,0.0 +0,1.437300,0.0,0.0,0.0,0.0,0.0 +0,1.437400,0.0,0.0,0.0,0.0,0.0 +0,1.437500,0.0,0.0,0.0,0.0,0.0 +0,1.437600,0.0,0.0,0.0,0.0,0.0 +0,1.437700,0.0,0.0,0.0,0.0,0.0 +0,1.437800,0.0,0.0,0.0,0.0,0.0 +0,1.437900,0.0,0.0,0.0,0.0,0.0 +0,1.438000,0.0,0.0,0.0,0.0,0.0 +0,1.438100,0.0,0.0,0.0,0.0,0.0 +0,1.438200,0.0,0.0,0.0,0.0,0.0 +0,1.438300,0.0,0.0,0.0,0.0,0.0 +0,1.438400,0.0,0.0,0.0,0.0,0.0 +0,1.438500,0.0,0.0,0.0,0.0,0.0 +0,1.438600,0.0,0.0,0.0,0.0,0.0 +0,1.438700,0.0,0.0,0.0,0.0,0.0 +0,1.438800,0.0,0.0,0.0,0.0,0.0 +0,1.438900,0.0,0.0,0.0,0.0,0.0 +0,1.439000,0.0,0.0,0.0,0.0,0.0 +0,1.439100,0.0,0.0,0.0,0.0,0.0 +0,1.439200,0.0,0.0,0.0,0.0,0.0 +0,1.439300,0.0,0.0,0.0,0.0,0.0 +0,1.439400,0.0,0.0,0.0,0.0,0.0 +0,1.439500,0.0,0.0,0.0,0.0,0.0 +0,1.439600,0.0,0.0,0.0,0.0,0.0 +0,1.439700,0.0,0.0,0.0,0.0,0.0 +0,1.439800,0.0,0.0,0.0,0.0,0.0 +0,1.439900,0.0,0.0,0.0,0.0,0.0 +0,1.440000,0.0,0.0,0.0,0.0,0.0 +0,1.440100,0.0,0.0,0.0,0.0,0.0 +1,1244.548839,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.440200,0.0,0.0,0.0,0.0,0.0 +0,1.440300,0.0,0.0,0.0,0.0,0.0 +0,1.440400,0.0,0.0,0.0,0.0,0.0 +0,1.440500,0.0,0.0,0.0,0.0,0.0 +0,1.440600,0.0,0.0,0.0,0.0,0.0 +0,1.440700,0.0,0.0,0.0,0.0,0.0 +0,1.440800,0.0,0.0,0.0,0.0,0.0 +0,1.440900,0.0,0.0,0.0,0.0,0.0 +0,1.441000,0.0,0.0,0.0,0.0,0.0 +0,1.441100,0.0,0.0,0.0,0.0,0.0 +0,1.441200,0.0,0.0,0.0,0.0,0.0 +0,1.441300,0.0,0.0,0.0,0.0,0.0 +0,1.441400,0.0,0.0,0.0,0.0,0.0 +0,1.441500,0.0,0.0,0.0,0.0,0.0 +0,1.441600,0.0,0.0,0.0,0.0,0.0 +0,1.441700,0.0,0.0,0.0,0.0,0.0 +0,1.441800,0.0,0.0,0.0,0.0,0.0 +0,1.441900,0.0,0.0,0.0,0.0,0.0 +0,1.442000,0.0,0.0,0.0,0.0,0.0 +0,1.442100,0.0,0.0,0.0,0.0,0.0 +0,1.442200,0.0,0.0,0.0,0.0,0.0 +0,1.442300,0.0,0.0,0.0,0.0,0.0 +0,1.442400,0.0,0.0,0.0,0.0,0.0 +0,1.442500,0.0,0.0,0.0,0.0,0.0 +0,1.442600,0.0,0.0,0.0,0.0,0.0 +0,1.442700,0.0,0.0,0.0,0.0,0.0 +0,1.442800,0.0,0.0,0.0,0.0,0.0 +0,1.442900,0.0,0.0,0.0,0.0,0.0 +0,1.443000,0.0,0.0,0.0,0.0,0.0 +0,1.443100,0.0,0.0,0.0,0.0,0.0 +0,1.443200,0.0,0.0,0.0,0.0,0.0 +0,1.443300,0.0,0.0,0.0,0.0,0.0 +0,1.443400,0.0,0.0,0.0,0.0,0.0 +0,1.443500,0.0,0.0,0.0,0.0,0.0 +0,1.443600,0.0,0.0,0.0,0.0,0.0 +0,1.443700,0.0,0.0,0.0,0.0,0.0 +0,1.443800,0.0,0.0,0.0,0.0,0.0 +0,1.443900,0.0,0.0,0.0,0.0,0.0 +0,1.444000,0.0,0.0,0.0,0.0,0.0 +0,1.444100,0.0,0.0,0.0,0.0,0.0 +0,1.444200,0.0,0.0,0.0,0.0,0.0 +0,1.444300,0.0,0.0,0.0,0.0,0.0 +0,1.444400,0.0,0.0,0.0,0.0,0.0 +0,1.444500,0.0,0.0,0.0,0.0,0.0 +0,1.444600,0.0,0.0,0.0,0.0,0.0 +0,1.444700,0.0,0.0,0.0,0.0,0.0 +0,1.444800,0.0,0.0,0.0,0.0,0.0 +0,1.444900,0.0,0.0,0.0,0.0,0.0 +0,1.445000,0.0,0.0,0.0,0.0,0.0 +0,1.445100,0.0,0.0,0.0,0.0,0.0 +0,1.445200,0.0,0.0,0.0,0.0,0.0 +0,1.445300,0.0,0.0,0.0,0.0,0.0 +0,1.445400,0.0,0.0,0.0,0.0,0.0 +0,1.445500,0.0,0.0,0.0,0.0,0.0 +0,1.445600,0.0,0.0,0.0,0.0,0.0 +0,1.445700,0.0,0.0,0.0,0.0,0.0 +0,1.445800,0.0,0.0,0.0,0.0,0.0 +0,1.445900,0.0,0.0,0.0,0.0,0.0 +0,1.446000,0.0,0.0,0.0,0.0,0.0 +0,1.446100,0.0,0.0,0.0,0.0,0.0 +0,1.446200,0.0,0.0,0.0,0.0,0.0 +0,1.446300,0.0,0.0,0.0,0.0,0.0 +0,1.446400,0.0,0.0,0.0,0.0,0.0 +0,1.446500,0.0,0.0,0.0,0.0,0.0 +0,1.446600,0.0,0.0,0.0,0.0,0.0 +0,1.446700,0.0,0.0,0.0,0.0,0.0 +0,1.446800,0.0,0.0,0.0,0.0,0.0 +0,1.446900,0.0,0.0,0.0,0.0,0.0 +0,1.447000,0.0,0.0,0.0,0.0,0.0 +0,1.447100,0.0,0.0,0.0,0.0,0.0 +0,1.447200,0.0,0.0,0.0,0.0,0.0 +0,1.447300,0.0,0.0,0.0,0.0,0.0 +0,1.447400,0.0,0.0,0.0,0.0,0.0 +0,1.447500,0.0,0.0,0.0,0.0,0.0 +0,1.447600,0.0,0.0,0.0,0.0,0.0 +0,1.447700,0.0,0.0,0.0,0.0,0.0 +0,1.447800,0.0,0.0,0.0,0.0,0.0 +0,1.447900,0.0,0.0,0.0,0.0,0.0 +0,1.448000,0.0,0.0,0.0,0.0,0.0 +0,1.448100,0.0,0.0,0.0,0.0,0.0 +0,1.448200,0.0,0.0,0.0,0.0,0.0 +0,1.448300,0.0,0.0,0.0,0.0,0.0 +0,1.448400,0.0,0.0,0.0,0.0,0.0 +0,1.448500,0.0,0.0,0.0,0.0,0.0 +0,1.448600,0.0,0.0,0.0,0.0,0.0 +0,1.448700,0.0,0.0,0.0,0.0,0.0 +0,1.448800,0.0,0.0,0.0,0.0,0.0 +0,1.448900,0.0,0.0,0.0,0.0,0.0 +0,1.449000,0.0,0.0,0.0,0.0,0.0 +0,1.449100,0.0,0.0,0.0,0.0,0.0 +0,1.449200,0.0,0.0,0.0,0.0,0.0 +0,1.449300,0.0,0.0,0.0,0.0,0.0 +0,1.449400,0.0,0.0,0.0,0.0,0.0 +0,1.449500,0.0,0.0,0.0,0.0,0.0 +0,1.449600,0.0,0.0,0.0,0.0,0.0 +0,1.449700,0.0,0.0,0.0,0.0,0.0 +0,1.449800,0.0,0.0,0.0,0.0,0.0 +0,1.449900,0.0,0.0,0.0,0.0,0.0 +0,1.450000,0.0,0.0,0.0,0.0,0.0 +0,1.450100,0.0,0.0,0.0,0.0,0.0 +0,1.450200,0.0,0.0,0.0,0.0,0.0 +0,1.450300,0.0,0.0,0.0,0.0,0.0 +0,1.450400,0.0,0.0,0.0,0.0,0.0 +0,1.450500,0.0,0.0,0.0,0.0,0.0 +0,1.450600,0.0,0.0,0.0,0.0,0.0 +0,1.450700,0.0,0.0,0.0,0.0,0.0 +0,1.450800,0.0,0.0,0.0,0.0,0.0 +0,1.450900,0.0,0.0,0.0,0.0,0.0 +0,1.451000,0.0,0.0,0.0,0.0,0.0 +0,1.451100,0.0,0.0,0.0,0.0,0.0 +0,1.451200,0.0,0.0,0.0,0.0,0.0 +0,1.451300,0.0,0.0,0.0,0.0,0.0 +0,1.451400,0.0,0.0,0.0,0.0,0.0 +0,1.451500,0.0,0.0,0.0,0.0,0.0 +0,1.451600,0.0,0.0,0.0,0.0,0.0 +0,1.451700,0.0,0.0,0.0,0.0,0.0 +0,1.451800,0.0,0.0,0.0,0.0,0.0 +0,1.451900,0.0,0.0,0.0,0.0,0.0 +0,1.452000,0.0,0.0,0.0,0.0,0.0 +0,1.452100,0.0,0.0,0.0,0.0,0.0 +0,1.452200,0.0,0.0,0.0,0.0,0.0 +0,1.452300,0.0,0.0,0.0,0.0,0.0 +0,1.452400,0.0,0.0,0.0,0.0,0.0 +0,1.452500,0.0,0.0,0.0,0.0,0.0 +0,1.452600,0.0,0.0,0.0,0.0,0.0 +0,1.452700,0.0,0.0,0.0,0.0,0.0 +0,1.452800,0.0,0.0,0.0,0.0,0.0 +0,1.452900,0.0,0.0,0.0,0.0,0.0 +0,1.453000,0.0,0.0,0.0,0.0,0.0 +0,1.453100,0.0,0.0,0.0,0.0,0.0 +0,1.453200,0.0,0.0,0.0,0.0,0.0 +0,1.453300,0.0,0.0,0.0,0.0,0.0 +0,1.453400,0.0,0.0,0.0,0.0,0.0 +0,1.453500,0.0,0.0,0.0,0.0,0.0 +0,1.453600,0.0,0.0,0.0,0.0,0.0 +0,1.453700,0.0,0.0,0.0,0.0,0.0 +0,1.453800,0.0,0.0,0.0,0.0,0.0 +0,1.453900,0.0,0.0,0.0,0.0,0.0 +0,1.454000,0.0,0.0,0.0,0.0,0.0 +0,1.454100,0.0,0.0,0.0,0.0,0.0 +0,1.454200,0.0,0.0,0.0,0.0,0.0 +0,1.454300,0.0,0.0,0.0,0.0,0.0 +0,1.454400,0.0,0.0,0.0,0.0,0.0 +0,1.454500,0.0,0.0,0.0,0.0,0.0 +0,1.454600,0.0,0.0,0.0,0.0,0.0 +0,1.454700,0.0,0.0,0.0,0.0,0.0 +0,1.454800,0.0,0.0,0.0,0.0,0.0 +0,1.454900,0.0,0.0,0.0,0.0,0.0 +0,1.455000,0.0,0.0,0.0,0.0,0.0 +0,1.455100,0.0,0.0,0.0,0.0,0.0 +0,1.455200,0.0,0.0,0.0,0.0,0.0 +0,1.455300,0.0,0.0,0.0,0.0,0.0 +0,1.455400,0.0,0.0,0.0,0.0,0.0 +0,1.455500,0.0,0.0,0.0,0.0,0.0 +0,1.455600,0.0,0.0,0.0,0.0,0.0 +0,1.455700,0.0,0.0,0.0,0.0,0.0 +0,1.455800,0.0,0.0,0.0,0.0,0.0 +0,1.455900,0.0,0.0,0.0,0.0,0.0 +0,1.456000,0.0,0.0,0.0,0.0,0.0 +0,1.456100,0.0,0.0,0.0,0.0,0.0 +0,1.456200,0.0,0.0,0.0,0.0,0.0 +0,1.456300,0.0,0.0,0.0,0.0,0.0 +0,1.456400,0.0,0.0,0.0,0.0,0.0 +0,1.456500,0.0,0.0,0.0,0.0,0.0 +0,1.456600,0.0,0.0,0.0,0.0,0.0 +0,1.456700,0.0,0.0,0.0,0.0,0.0 +0,1.456800,0.0,0.0,0.0,0.0,0.0 +0,1.456900,0.0,0.0,0.0,0.0,0.0 +0,1.457000,0.0,0.0,0.0,0.0,0.0 +0,1.457100,0.0,0.0,0.0,0.0,0.0 +0,1.457200,0.0,0.0,0.0,0.0,0.0 +0,1.457300,0.0,0.0,0.0,0.0,0.0 +0,1.457400,0.0,0.0,0.0,0.0,0.0 +0,1.457500,0.0,0.0,0.0,0.0,0.0 +0,1.457600,0.0,0.0,0.0,0.0,0.0 +0,1.457700,0.0,0.0,0.0,0.0,0.0 +0,1.457800,0.0,0.0,0.0,0.0,0.0 +0,1.457900,0.0,0.0,0.0,0.0,0.0 +0,1.458000,0.0,0.0,0.0,0.0,0.0 +0,1.458100,0.0,0.0,0.0,0.0,0.0 +0,1.458200,0.0,0.0,0.0,0.0,0.0 +0,1.458300,0.0,0.0,0.0,0.0,0.0 +0,1.458400,0.0,0.0,0.0,0.0,0.0 +0,1.458500,0.0,0.0,0.0,0.0,0.0 +0,1.458600,0.0,0.0,0.0,0.0,0.0 +0,1.458700,0.0,0.0,0.0,0.0,0.0 +0,1.458800,0.0,0.0,0.0,0.0,0.0 +0,1.458900,0.0,0.0,0.0,0.0,0.0 +0,1.459000,0.0,0.0,0.0,0.0,0.0 +0,1.459100,0.0,0.0,0.0,0.0,0.0 +0,1.459200,0.0,0.0,0.0,0.0,0.0 +0,1.459300,0.0,0.0,0.0,0.0,0.0 +0,1.459400,0.0,0.0,0.0,0.0,0.0 +0,1.459500,0.0,0.0,0.0,0.0,0.0 +0,1.459600,0.0,0.0,0.0,0.0,0.0 +0,1.459700,0.0,0.0,0.0,0.0,0.0 +0,1.459800,0.0,0.0,0.0,0.0,0.0 +0,1.459900,0.0,0.0,0.0,0.0,0.0 +0,1.460000,0.0,0.0,0.0,0.0,0.0 +0,1.460100,0.0,0.0,0.0,0.0,0.0 +1,1297.123048,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.460200,0.0,0.0,0.0,0.0,0.0 +0,1.460300,0.0,0.0,0.0,0.0,0.0 +0,1.460400,0.0,0.0,0.0,0.0,0.0 +0,1.460500,0.0,0.0,0.0,0.0,0.0 +0,1.460600,0.0,0.0,0.0,0.0,0.0 +0,1.460700,0.0,0.0,0.0,0.0,0.0 +0,1.460800,0.0,0.0,0.0,0.0,0.0 +0,1.460900,0.0,0.0,0.0,0.0,0.0 +0,1.461000,0.0,0.0,0.0,0.0,0.0 +0,1.461100,0.0,0.0,0.0,0.0,0.0 +0,1.461200,0.0,0.0,0.0,0.0,0.0 +0,1.461300,0.0,0.0,0.0,0.0,0.0 +0,1.461400,0.0,0.0,0.0,0.0,0.0 +0,1.461500,0.0,0.0,0.0,0.0,0.0 +0,1.461600,0.0,0.0,0.0,0.0,0.0 +0,1.461700,0.0,0.0,0.0,0.0,0.0 +0,1.461800,0.0,0.0,0.0,0.0,0.0 +0,1.461900,0.0,0.0,0.0,0.0,0.0 +0,1.462000,0.0,0.0,0.0,0.0,0.0 +0,1.462100,0.0,0.0,0.0,0.0,0.0 +0,1.462200,0.0,0.0,0.0,0.0,0.0 +0,1.462300,0.0,0.0,0.0,0.0,0.0 +0,1.462400,0.0,0.0,0.0,0.0,0.0 +0,1.462500,0.0,0.0,0.0,0.0,0.0 +0,1.462600,0.0,0.0,0.0,0.0,0.0 +0,1.462700,0.0,0.0,0.0,0.0,0.0 +0,1.462800,0.0,0.0,0.0,0.0,0.0 +0,1.462900,0.0,0.0,0.0,0.0,0.0 +0,1.463000,0.0,0.0,0.0,0.0,0.0 +0,1.463100,0.0,0.0,0.0,0.0,0.0 +0,1.463200,0.0,0.0,0.0,0.0,0.0 +0,1.463300,0.0,0.0,0.0,0.0,0.0 +0,1.463400,0.0,0.0,0.0,0.0,0.0 +0,1.463500,0.0,0.0,0.0,0.0,0.0 +0,1.463600,0.0,0.0,0.0,0.0,0.0 +0,1.463700,0.0,0.0,0.0,0.0,0.0 +0,1.463800,0.0,0.0,0.0,0.0,0.0 +0,1.463900,0.0,0.0,0.0,0.0,0.0 +0,1.464000,0.0,0.0,0.0,0.0,0.0 +0,1.464100,0.0,0.0,0.0,0.0,0.0 +0,1.464200,0.0,0.0,0.0,0.0,0.0 +0,1.464300,0.0,0.0,0.0,0.0,0.0 +0,1.464400,0.0,0.0,0.0,0.0,0.0 +0,1.464500,0.0,0.0,0.0,0.0,0.0 +0,1.464600,0.0,0.0,0.0,0.0,0.0 +0,1.464700,0.0,0.0,0.0,0.0,0.0 +0,1.464800,0.0,0.0,0.0,0.0,0.0 +0,1.464900,0.0,0.0,0.0,0.0,0.0 +0,1.465000,0.0,0.0,0.0,0.0,0.0 +0,1.465100,0.0,0.0,0.0,0.0,0.0 +0,1.465200,0.0,0.0,0.0,0.0,0.0 +0,1.465300,0.0,0.0,0.0,0.0,0.0 +0,1.465400,0.0,0.0,0.0,0.0,0.0 +0,1.465500,0.0,0.0,0.0,0.0,0.0 +0,1.465600,0.0,0.0,0.0,0.0,0.0 +0,1.465700,0.0,0.0,0.0,0.0,0.0 +0,1.465800,0.0,0.0,0.0,0.0,0.0 +0,1.465900,0.0,0.0,0.0,0.0,0.0 +0,1.466000,0.0,0.0,0.0,0.0,0.0 +0,1.466100,0.0,0.0,0.0,0.0,0.0 +0,1.466200,0.0,0.0,0.0,0.0,0.0 +0,1.466300,0.0,0.0,0.0,0.0,0.0 +0,1.466400,0.0,0.0,0.0,0.0,0.0 +0,1.466500,0.0,0.0,0.0,0.0,0.0 +0,1.466600,0.0,0.0,0.0,0.0,0.0 +0,1.466700,0.0,0.0,0.0,0.0,0.0 +0,1.466800,0.0,0.0,0.0,0.0,0.0 +0,1.466900,0.0,0.0,0.0,0.0,0.0 +0,1.467000,0.0,0.0,0.0,0.0,0.0 +0,1.467100,0.0,0.0,0.0,0.0,0.0 +0,1.467200,0.0,0.0,0.0,0.0,0.0 +0,1.467300,0.0,0.0,0.0,0.0,0.0 +0,1.467400,0.0,0.0,0.0,0.0,0.0 +0,1.467500,0.0,0.0,0.0,0.0,0.0 +0,1.467600,0.0,0.0,0.0,0.0,0.0 +0,1.467700,0.0,0.0,0.0,0.0,0.0 +0,1.467800,0.0,0.0,0.0,0.0,0.0 +0,1.467900,0.0,0.0,0.0,0.0,0.0 +0,1.468000,0.0,0.0,0.0,0.0,0.0 +0,1.468100,0.0,0.0,0.0,0.0,0.0 +0,1.468200,0.0,0.0,0.0,0.0,0.0 +0,1.468300,0.0,0.0,0.0,0.0,0.0 +0,1.468400,0.0,0.0,0.0,0.0,0.0 +0,1.468500,0.0,0.0,0.0,0.0,0.0 +0,1.468600,0.0,0.0,0.0,0.0,0.0 +0,1.468700,0.0,0.0,0.0,0.0,0.0 +0,1.468800,0.0,0.0,0.0,0.0,0.0 +0,1.468900,0.0,0.0,0.0,0.0,0.0 +0,1.469000,0.0,0.0,0.0,0.0,0.0 +0,1.469100,0.0,0.0,0.0,0.0,0.0 +0,1.469200,0.0,0.0,0.0,0.0,0.0 +0,1.469300,0.0,0.0,0.0,0.0,0.0 +0,1.469400,0.0,0.0,0.0,0.0,0.0 +0,1.469500,0.0,0.0,0.0,0.0,0.0 +0,1.469600,0.0,0.0,0.0,0.0,0.0 +0,1.469700,0.0,0.0,0.0,0.0,0.0 +0,1.469800,0.0,0.0,0.0,0.0,0.0 +0,1.469900,0.0,0.0,0.0,0.0,0.0 +0,1.470000,0.0,0.0,0.0,0.0,0.0 +0,1.470100,0.0,0.0,0.0,0.0,0.0 +0,1.470200,0.0,0.0,0.0,0.0,0.0 +0,1.470300,0.0,0.0,0.0,0.0,0.0 +0,1.470400,0.0,0.0,0.0,0.0,0.0 +0,1.470500,0.0,0.0,0.0,0.0,0.0 +0,1.470600,0.0,0.0,0.0,0.0,0.0 +0,1.470700,0.0,0.0,0.0,0.0,0.0 +0,1.470800,0.0,0.0,0.0,0.0,0.0 +0,1.470900,0.0,0.0,0.0,0.0,0.0 +0,1.471000,0.0,0.0,0.0,0.0,0.0 +0,1.471100,0.0,0.0,0.0,0.0,0.0 +0,1.471200,0.0,0.0,0.0,0.0,0.0 +0,1.471300,0.0,0.0,0.0,0.0,0.0 +0,1.471400,0.0,0.0,0.0,0.0,0.0 +0,1.471500,0.0,0.0,0.0,0.0,0.0 +0,1.471600,0.0,0.0,0.0,0.0,0.0 +0,1.471700,0.0,0.0,0.0,0.0,0.0 +0,1.471800,0.0,0.0,0.0,0.0,0.0 +0,1.471900,0.0,0.0,0.0,0.0,0.0 +0,1.472000,0.0,0.0,0.0,0.0,0.0 +0,1.472100,0.0,0.0,0.0,0.0,0.0 +0,1.472200,0.0,0.0,0.0,0.0,0.0 +0,1.472300,0.0,0.0,0.0,0.0,0.0 +0,1.472400,0.0,0.0,0.0,0.0,0.0 +0,1.472500,0.0,0.0,0.0,0.0,0.0 +0,1.472600,0.0,0.0,0.0,0.0,0.0 +0,1.472700,0.0,0.0,0.0,0.0,0.0 +0,1.472800,0.0,0.0,0.0,0.0,0.0 +0,1.472900,0.0,0.0,0.0,0.0,0.0 +0,1.473000,0.0,0.0,0.0,0.0,0.0 +0,1.473100,0.0,0.0,0.0,0.0,0.0 +0,1.473200,0.0,0.0,0.0,0.0,0.0 +0,1.473300,0.0,0.0,0.0,0.0,0.0 +0,1.473400,0.0,0.0,0.0,0.0,0.0 +0,1.473500,0.0,0.0,0.0,0.0,0.0 +0,1.473600,0.0,0.0,0.0,0.0,0.0 +0,1.473700,0.0,0.0,0.0,0.0,0.0 +0,1.473800,0.0,0.0,0.0,0.0,0.0 +0,1.473900,0.0,0.0,0.0,0.0,0.0 +0,1.474000,0.0,0.0,0.0,0.0,0.0 +0,1.474100,0.0,0.0,0.0,0.0,0.0 +0,1.474200,0.0,0.0,0.0,0.0,0.0 +0,1.474300,0.0,0.0,0.0,0.0,0.0 +0,1.474400,0.0,0.0,0.0,0.0,0.0 +0,1.474500,0.0,0.0,0.0,0.0,0.0 +0,1.474600,0.0,0.0,0.0,0.0,0.0 +0,1.474700,0.0,0.0,0.0,0.0,0.0 +0,1.474800,0.0,0.0,0.0,0.0,0.0 +0,1.474900,0.0,0.0,0.0,0.0,0.0 +0,1.475000,0.0,0.0,0.0,0.0,0.0 +0,1.475100,0.0,0.0,0.0,0.0,0.0 +0,1.475200,0.0,0.0,0.0,0.0,0.0 +0,1.475300,0.0,0.0,0.0,0.0,0.0 +0,1.475400,0.0,0.0,0.0,0.0,0.0 +0,1.475500,0.0,0.0,0.0,0.0,0.0 +0,1.475600,0.0,0.0,0.0,0.0,0.0 +0,1.475700,0.0,0.0,0.0,0.0,0.0 +0,1.475800,0.0,0.0,0.0,0.0,0.0 +0,1.475900,0.0,0.0,0.0,0.0,0.0 +0,1.476000,0.0,0.0,0.0,0.0,0.0 +0,1.476100,0.0,0.0,0.0,0.0,0.0 +0,1.476200,0.0,0.0,0.0,0.0,0.0 +0,1.476300,0.0,0.0,0.0,0.0,0.0 +0,1.476400,0.0,0.0,0.0,0.0,0.0 +0,1.476500,0.0,0.0,0.0,0.0,0.0 +0,1.476600,0.0,0.0,0.0,0.0,0.0 +0,1.476700,0.0,0.0,0.0,0.0,0.0 +0,1.476800,0.0,0.0,0.0,0.0,0.0 +0,1.476900,0.0,0.0,0.0,0.0,0.0 +0,1.477000,0.0,0.0,0.0,0.0,0.0 +0,1.477100,0.0,0.0,0.0,0.0,0.0 +0,1.477200,0.0,0.0,0.0,0.0,0.0 +0,1.477300,0.0,0.0,0.0,0.0,0.0 +0,1.477400,0.0,0.0,0.0,0.0,0.0 +0,1.477500,0.0,0.0,0.0,0.0,0.0 +0,1.477600,0.0,0.0,0.0,0.0,0.0 +0,1.477700,0.0,0.0,0.0,0.0,0.0 +0,1.477800,0.0,0.0,0.0,0.0,0.0 +0,1.477900,0.0,0.0,0.0,0.0,0.0 +0,1.478000,0.0,0.0,0.0,0.0,0.0 +0,1.478100,0.0,0.0,0.0,0.0,0.0 +0,1.478200,0.0,0.0,0.0,0.0,0.0 +0,1.478300,0.0,0.0,0.0,0.0,0.0 +0,1.478400,0.0,0.0,0.0,0.0,0.0 +0,1.478500,0.0,0.0,0.0,0.0,0.0 +0,1.478600,0.0,0.0,0.0,0.0,0.0 +0,1.478700,0.0,0.0,0.0,0.0,0.0 +0,1.478800,0.0,0.0,0.0,0.0,0.0 +0,1.478900,0.0,0.0,0.0,0.0,0.0 +0,1.479000,0.0,0.0,0.0,0.0,0.0 +0,1.479100,0.0,0.0,0.0,0.0,0.0 +0,1.479200,0.0,0.0,0.0,0.0,0.0 +0,1.479300,0.0,0.0,0.0,0.0,0.0 +0,1.479400,0.0,0.0,0.0,0.0,0.0 +0,1.479500,0.0,0.0,0.0,0.0,0.0 +0,1.479600,0.0,0.0,0.0,0.0,0.0 +0,1.479700,0.0,0.0,0.0,0.0,0.0 +0,1.479800,0.0,0.0,0.0,0.0,0.0 +0,1.479900,0.0,0.0,0.0,0.0,0.0 +0,1.480000,0.0,0.0,0.0,0.0,0.0 +0,1.480100,0.0,0.0,0.0,0.0,0.0 +1,1351.157407,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.480200,0.0,0.0,0.0,0.0,0.0 +0,1.480300,0.0,0.0,0.0,0.0,0.0 +0,1.480400,0.0,0.0,0.0,0.0,0.0 +0,1.480500,0.0,0.0,0.0,0.0,0.0 +0,1.480600,0.0,0.0,0.0,0.0,0.0 +0,1.480700,0.0,0.0,0.0,0.0,0.0 +0,1.480800,0.0,0.0,0.0,0.0,0.0 +0,1.480900,0.0,0.0,0.0,0.0,0.0 +0,1.481000,0.0,0.0,0.0,0.0,0.0 +0,1.481100,0.0,0.0,0.0,0.0,0.0 +0,1.481200,0.0,0.0,0.0,0.0,0.0 +0,1.481300,0.0,0.0,0.0,0.0,0.0 +0,1.481400,0.0,0.0,0.0,0.0,0.0 +0,1.481500,0.0,0.0,0.0,0.0,0.0 +0,1.481600,0.0,0.0,0.0,0.0,0.0 +0,1.481700,0.0,0.0,0.0,0.0,0.0 +0,1.481800,0.0,0.0,0.0,0.0,0.0 +0,1.481900,0.0,0.0,0.0,0.0,0.0 +0,1.482000,0.0,0.0,0.0,0.0,0.0 +0,1.482100,0.0,0.0,0.0,0.0,0.0 +0,1.482200,0.0,0.0,0.0,0.0,0.0 +0,1.482300,0.0,0.0,0.0,0.0,0.0 +0,1.482400,0.0,0.0,0.0,0.0,0.0 +0,1.482500,0.0,0.0,0.0,0.0,0.0 +0,1.482600,0.0,0.0,0.0,0.0,0.0 +0,1.482700,0.0,0.0,0.0,0.0,0.0 +0,1.482800,0.0,0.0,0.0,0.0,0.0 +0,1.482900,0.0,0.0,0.0,0.0,0.0 +0,1.483000,0.0,0.0,0.0,0.0,0.0 +0,1.483100,0.0,0.0,0.0,0.0,0.0 +0,1.483200,0.0,0.0,0.0,0.0,0.0 +0,1.483300,0.0,0.0,0.0,0.0,0.0 +0,1.483400,0.0,0.0,0.0,0.0,0.0 +0,1.483500,0.0,0.0,0.0,0.0,0.0 +0,1.483600,0.0,0.0,0.0,0.0,0.0 +0,1.483700,0.0,0.0,0.0,0.0,0.0 +0,1.483800,0.0,0.0,0.0,0.0,0.0 +0,1.483900,0.0,0.0,0.0,0.0,0.0 +0,1.484000,0.0,0.0,0.0,0.0,0.0 +0,1.484100,0.0,0.0,0.0,0.0,0.0 +0,1.484200,0.0,0.0,0.0,0.0,0.0 +0,1.484300,0.0,0.0,0.0,0.0,0.0 +0,1.484400,0.0,0.0,0.0,0.0,0.0 +0,1.484500,0.0,0.0,0.0,0.0,0.0 +0,1.484600,0.0,0.0,0.0,0.0,0.0 +0,1.484700,0.0,0.0,0.0,0.0,0.0 +0,1.484800,0.0,0.0,0.0,0.0,0.0 +0,1.484900,0.0,0.0,0.0,0.0,0.0 +0,1.485000,0.0,0.0,0.0,0.0,0.0 +0,1.485100,0.0,0.0,0.0,0.0,0.0 +0,1.485200,0.0,0.0,0.0,0.0,0.0 +0,1.485300,0.0,0.0,0.0,0.0,0.0 +0,1.485400,0.0,0.0,0.0,0.0,0.0 +0,1.485500,0.0,0.0,0.0,0.0,0.0 +0,1.485600,0.0,0.0,0.0,0.0,0.0 +0,1.485700,0.0,0.0,0.0,0.0,0.0 +0,1.485800,0.0,0.0,0.0,0.0,0.0 +0,1.485900,0.0,0.0,0.0,0.0,0.0 +0,1.486000,0.0,0.0,0.0,0.0,0.0 +0,1.486100,0.0,0.0,0.0,0.0,0.0 +0,1.486200,0.0,0.0,0.0,0.0,0.0 +0,1.486300,0.0,0.0,0.0,0.0,0.0 +0,1.486400,0.0,0.0,0.0,0.0,0.0 +0,1.486500,0.0,0.0,0.0,0.0,0.0 +0,1.486600,0.0,0.0,0.0,0.0,0.0 +0,1.486700,0.0,0.0,0.0,0.0,0.0 +0,1.486800,0.0,0.0,0.0,0.0,0.0 +0,1.486900,0.0,0.0,0.0,0.0,0.0 +0,1.487000,0.0,0.0,0.0,0.0,0.0 +0,1.487100,0.0,0.0,0.0,0.0,0.0 +0,1.487200,0.0,0.0,0.0,0.0,0.0 +0,1.487300,0.0,0.0,0.0,0.0,0.0 +0,1.487400,0.0,0.0,0.0,0.0,0.0 +0,1.487500,0.0,0.0,0.0,0.0,0.0 +0,1.487600,0.0,0.0,0.0,0.0,0.0 +0,1.487700,0.0,0.0,0.0,0.0,0.0 +0,1.487800,0.0,0.0,0.0,0.0,0.0 +0,1.487900,0.0,0.0,0.0,0.0,0.0 +0,1.488000,0.0,0.0,0.0,0.0,0.0 +0,1.488100,0.0,0.0,0.0,0.0,0.0 +0,1.488200,0.0,0.0,0.0,0.0,0.0 +0,1.488300,0.0,0.0,0.0,0.0,0.0 +0,1.488400,0.0,0.0,0.0,0.0,0.0 +0,1.488500,0.0,0.0,0.0,0.0,0.0 +0,1.488600,0.0,0.0,0.0,0.0,0.0 +0,1.488700,0.0,0.0,0.0,0.0,0.0 +0,1.488800,0.0,0.0,0.0,0.0,0.0 +0,1.488900,0.0,0.0,0.0,0.0,0.0 +0,1.489000,0.0,0.0,0.0,0.0,0.0 +0,1.489100,0.0,0.0,0.0,0.0,0.0 +0,1.489200,0.0,0.0,0.0,0.0,0.0 +0,1.489300,0.0,0.0,0.0,0.0,0.0 +0,1.489400,0.0,0.0,0.0,0.0,0.0 +0,1.489500,0.0,0.0,0.0,0.0,0.0 +0,1.489600,0.0,0.0,0.0,0.0,0.0 +0,1.489700,0.0,0.0,0.0,0.0,0.0 +0,1.489800,0.0,0.0,0.0,0.0,0.0 +0,1.489900,0.0,0.0,0.0,0.0,0.0 +0,1.490000,0.0,0.0,0.0,0.0,0.0 +0,1.490100,0.0,0.0,0.0,0.0,0.0 +0,1.490200,0.0,0.0,0.0,0.0,0.0 +0,1.490300,0.0,0.0,0.0,0.0,0.0 +0,1.490400,0.0,0.0,0.0,0.0,0.0 +0,1.490500,0.0,0.0,0.0,0.0,0.0 +0,1.490600,0.0,0.0,0.0,0.0,0.0 +0,1.490700,0.0,0.0,0.0,0.0,0.0 +0,1.490800,0.0,0.0,0.0,0.0,0.0 +0,1.490900,0.0,0.0,0.0,0.0,0.0 +0,1.491000,0.0,0.0,0.0,0.0,0.0 +0,1.491100,0.0,0.0,0.0,0.0,0.0 +0,1.491200,0.0,0.0,0.0,0.0,0.0 +0,1.491300,0.0,0.0,0.0,0.0,0.0 +0,1.491400,0.0,0.0,0.0,0.0,0.0 +0,1.491500,0.0,0.0,0.0,0.0,0.0 +0,1.491600,0.0,0.0,0.0,0.0,0.0 +0,1.491700,0.0,0.0,0.0,0.0,0.0 +0,1.491800,0.0,0.0,0.0,0.0,0.0 +0,1.491900,0.0,0.0,0.0,0.0,0.0 +0,1.492000,0.0,0.0,0.0,0.0,0.0 +0,1.492100,0.0,0.0,0.0,0.0,0.0 +0,1.492200,0.0,0.0,0.0,0.0,0.0 +0,1.492300,0.0,0.0,0.0,0.0,0.0 +0,1.492400,0.0,0.0,0.0,0.0,0.0 +0,1.492500,0.0,0.0,0.0,0.0,0.0 +0,1.492600,0.0,0.0,0.0,0.0,0.0 +0,1.492700,0.0,0.0,0.0,0.0,0.0 +0,1.492800,0.0,0.0,0.0,0.0,0.0 +0,1.492900,0.0,0.0,0.0,0.0,0.0 +0,1.493000,0.0,0.0,0.0,0.0,0.0 +0,1.493100,0.0,0.0,0.0,0.0,0.0 +0,1.493200,0.0,0.0,0.0,0.0,0.0 +0,1.493300,0.0,0.0,0.0,0.0,0.0 +0,1.493400,0.0,0.0,0.0,0.0,0.0 +0,1.493500,0.0,0.0,0.0,0.0,0.0 +0,1.493600,0.0,0.0,0.0,0.0,0.0 +0,1.493700,0.0,0.0,0.0,0.0,0.0 +0,1.493800,0.0,0.0,0.0,0.0,0.0 +0,1.493900,0.0,0.0,0.0,0.0,0.0 +0,1.494000,0.0,0.0,0.0,0.0,0.0 +0,1.494100,0.0,0.0,0.0,0.0,0.0 +0,1.494200,0.0,0.0,0.0,0.0,0.0 +0,1.494300,0.0,0.0,0.0,0.0,0.0 +0,1.494400,0.0,0.0,0.0,0.0,0.0 +0,1.494500,0.0,0.0,0.0,0.0,0.0 +0,1.494600,0.0,0.0,0.0,0.0,0.0 +0,1.494700,0.0,0.0,0.0,0.0,0.0 +0,1.494800,0.0,0.0,0.0,0.0,0.0 +0,1.494900,0.0,0.0,0.0,0.0,0.0 +0,1.495000,0.0,0.0,0.0,0.0,0.0 +0,1.495100,0.0,0.0,0.0,0.0,0.0 +0,1.495200,0.0,0.0,0.0,0.0,0.0 +0,1.495300,0.0,0.0,0.0,0.0,0.0 +0,1.495400,0.0,0.0,0.0,0.0,0.0 +0,1.495500,0.0,0.0,0.0,0.0,0.0 +0,1.495600,0.0,0.0,0.0,0.0,0.0 +0,1.495700,0.0,0.0,0.0,0.0,0.0 +0,1.495800,0.0,0.0,0.0,0.0,0.0 +0,1.495900,0.0,0.0,0.0,0.0,0.0 +0,1.496000,0.0,0.0,0.0,0.0,0.0 +0,1.496100,0.0,0.0,0.0,0.0,0.0 +0,1.496200,0.0,0.0,0.0,0.0,0.0 +0,1.496300,0.0,0.0,0.0,0.0,0.0 +0,1.496400,0.0,0.0,0.0,0.0,0.0 +0,1.496500,0.0,0.0,0.0,0.0,0.0 +0,1.496600,0.0,0.0,0.0,0.0,0.0 +0,1.496700,0.0,0.0,0.0,0.0,0.0 +0,1.496800,0.0,0.0,0.0,0.0,0.0 +0,1.496900,0.0,0.0,0.0,0.0,0.0 +0,1.497000,0.0,0.0,0.0,0.0,0.0 +0,1.497100,0.0,0.0,0.0,0.0,0.0 +0,1.497200,0.0,0.0,0.0,0.0,0.0 +0,1.497300,0.0,0.0,0.0,0.0,0.0 +0,1.497400,0.0,0.0,0.0,0.0,0.0 +0,1.497500,0.0,0.0,0.0,0.0,0.0 +0,1.497600,0.0,0.0,0.0,0.0,0.0 +0,1.497700,0.0,0.0,0.0,0.0,0.0 +0,1.497800,0.0,0.0,0.0,0.0,0.0 +0,1.497900,0.0,0.0,0.0,0.0,0.0 +0,1.498000,0.0,0.0,0.0,0.0,0.0 +0,1.498100,0.0,0.0,0.0,0.0,0.0 +0,1.498200,0.0,0.0,0.0,0.0,0.0 +0,1.498300,0.0,0.0,0.0,0.0,0.0 +0,1.498400,0.0,0.0,0.0,0.0,0.0 +0,1.498500,0.0,0.0,0.0,0.0,0.0 +0,1.498600,0.0,0.0,0.0,0.0,0.0 +0,1.498700,0.0,0.0,0.0,0.0,0.0 +0,1.498800,0.0,0.0,0.0,0.0,0.0 +0,1.498900,0.0,0.0,0.0,0.0,0.0 +0,1.499000,0.0,0.0,0.0,0.0,0.0 +0,1.499100,0.0,0.0,0.0,0.0,0.0 +0,1.499200,0.0,0.0,0.0,0.0,0.0 +0,1.499300,0.0,0.0,0.0,0.0,0.0 +0,1.499400,0.0,0.0,0.0,0.0,0.0 +0,1.499500,0.0,0.0,0.0,0.0,0.0 +0,1.499600,0.0,0.0,0.0,0.0,0.0 +0,1.499700,0.0,0.0,0.0,0.0,0.0 +0,1.499800,0.0,0.0,0.0,0.0,0.0 +0,1.499900,0.0,0.0,0.0,0.0,0.0 +0,1.500000,0.0,0.0,0.0,0.0,0.0 +0,1.500100,0.0,0.0,0.0,0.0,0.0 +1,1406.671916,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.500200,0.0,0.0,0.0,0.0,0.0 +0,1.500300,0.0,0.0,0.0,0.0,0.0 +0,1.500400,0.0,0.0,0.0,0.0,0.0 +0,1.500500,0.0,0.0,0.0,0.0,0.0 +0,1.500600,0.0,0.0,0.0,0.0,0.0 +0,1.500700,0.0,0.0,0.0,0.0,0.0 +0,1.500800,0.0,0.0,0.0,0.0,0.0 +0,1.500900,0.0,0.0,0.0,0.0,0.0 +0,1.501000,0.0,0.0,0.0,0.0,0.0 +0,1.501100,0.0,0.0,0.0,0.0,0.0 +0,1.501200,0.0,0.0,0.0,0.0,0.0 +0,1.501300,0.0,0.0,0.0,0.0,0.0 +0,1.501400,0.0,0.0,0.0,0.0,0.0 +0,1.501500,0.0,0.0,0.0,0.0,0.0 +0,1.501600,0.0,0.0,0.0,0.0,0.0 +0,1.501700,0.0,0.0,0.0,0.0,0.0 +0,1.501800,0.0,0.0,0.0,0.0,0.0 +0,1.501900,0.0,0.0,0.0,0.0,0.0 +0,1.502000,0.0,0.0,0.0,0.0,0.0 +0,1.502100,0.0,0.0,0.0,0.0,0.0 +0,1.502200,0.0,0.0,0.0,0.0,0.0 +0,1.502300,0.0,0.0,0.0,0.0,0.0 +0,1.502400,0.0,0.0,0.0,0.0,0.0 +0,1.502500,0.0,0.0,0.0,0.0,0.0 +0,1.502600,0.0,0.0,0.0,0.0,0.0 +0,1.502700,0.0,0.0,0.0,0.0,0.0 +0,1.502800,0.0,0.0,0.0,0.0,0.0 +0,1.502900,0.0,0.0,0.0,0.0,0.0 +0,1.503000,0.0,0.0,0.0,0.0,0.0 +0,1.503100,0.0,0.0,0.0,0.0,0.0 +0,1.503200,0.0,0.0,0.0,0.0,0.0 +0,1.503300,0.0,0.0,0.0,0.0,0.0 +0,1.503400,0.0,0.0,0.0,0.0,0.0 +0,1.503500,0.0,0.0,0.0,0.0,0.0 +0,1.503600,0.0,0.0,0.0,0.0,0.0 +0,1.503700,0.0,0.0,0.0,0.0,0.0 +0,1.503800,0.0,0.0,0.0,0.0,0.0 +0,1.503900,0.0,0.0,0.0,0.0,0.0 +0,1.504000,0.0,0.0,0.0,0.0,0.0 +0,1.504100,0.0,0.0,0.0,0.0,0.0 +0,1.504200,0.0,0.0,0.0,0.0,0.0 +0,1.504300,0.0,0.0,0.0,0.0,0.0 +0,1.504400,0.0,0.0,0.0,0.0,0.0 +0,1.504500,0.0,0.0,0.0,0.0,0.0 +0,1.504600,0.0,0.0,0.0,0.0,0.0 +0,1.504700,0.0,0.0,0.0,0.0,0.0 +0,1.504800,0.0,0.0,0.0,0.0,0.0 +0,1.504900,0.0,0.0,0.0,0.0,0.0 +0,1.505000,0.0,0.0,0.0,0.0,0.0 +0,1.505100,0.0,0.0,0.0,0.0,0.0 +0,1.505200,0.0,0.0,0.0,0.0,0.0 +0,1.505300,0.0,0.0,0.0,0.0,0.0 +0,1.505400,0.0,0.0,0.0,0.0,0.0 +0,1.505500,0.0,0.0,0.0,0.0,0.0 +0,1.505600,0.0,0.0,0.0,0.0,0.0 +0,1.505700,0.0,0.0,0.0,0.0,0.0 +0,1.505800,0.0,0.0,0.0,0.0,0.0 +0,1.505900,0.0,0.0,0.0,0.0,0.0 +0,1.506000,0.0,0.0,0.0,0.0,0.0 +0,1.506100,0.0,0.0,0.0,0.0,0.0 +0,1.506200,0.0,0.0,0.0,0.0,0.0 +0,1.506300,0.0,0.0,0.0,0.0,0.0 +0,1.506400,0.0,0.0,0.0,0.0,0.0 +0,1.506500,0.0,0.0,0.0,0.0,0.0 +0,1.506600,0.0,0.0,0.0,0.0,0.0 +0,1.506700,0.0,0.0,0.0,0.0,0.0 +0,1.506800,0.0,0.0,0.0,0.0,0.0 +0,1.506900,0.0,0.0,0.0,0.0,0.0 +0,1.507000,0.0,0.0,0.0,0.0,0.0 +0,1.507100,0.0,0.0,0.0,0.0,0.0 +0,1.507200,0.0,0.0,0.0,0.0,0.0 +0,1.507300,0.0,0.0,0.0,0.0,0.0 +0,1.507400,0.0,0.0,0.0,0.0,0.0 +0,1.507500,0.0,0.0,0.0,0.0,0.0 +0,1.507600,0.0,0.0,0.0,0.0,0.0 +0,1.507700,0.0,0.0,0.0,0.0,0.0 +0,1.507800,0.0,0.0,0.0,0.0,0.0 +0,1.507900,0.0,0.0,0.0,0.0,0.0 +0,1.508000,0.0,0.0,0.0,0.0,0.0 +0,1.508100,0.0,0.0,0.0,0.0,0.0 +0,1.508200,0.0,0.0,0.0,0.0,0.0 +0,1.508300,0.0,0.0,0.0,0.0,0.0 +0,1.508400,0.0,0.0,0.0,0.0,0.0 +0,1.508500,0.0,0.0,0.0,0.0,0.0 +0,1.508600,0.0,0.0,0.0,0.0,0.0 +0,1.508700,0.0,0.0,0.0,0.0,0.0 +0,1.508800,0.0,0.0,0.0,0.0,0.0 +0,1.508900,0.0,0.0,0.0,0.0,0.0 +0,1.509000,0.0,0.0,0.0,0.0,0.0 +0,1.509100,0.0,0.0,0.0,0.0,0.0 +0,1.509200,0.0,0.0,0.0,0.0,0.0 +0,1.509300,0.0,0.0,0.0,0.0,0.0 +0,1.509400,0.0,0.0,0.0,0.0,0.0 +0,1.509500,0.0,0.0,0.0,0.0,0.0 +0,1.509600,0.0,0.0,0.0,0.0,0.0 +0,1.509700,0.0,0.0,0.0,0.0,0.0 +0,1.509800,0.0,0.0,0.0,0.0,0.0 +0,1.509900,0.0,0.0,0.0,0.0,0.0 +0,1.510000,0.0,0.0,0.0,0.0,0.0 +0,1.510100,0.0,0.0,0.0,0.0,0.0 +0,1.510200,0.0,0.0,0.0,0.0,0.0 +0,1.510300,0.0,0.0,0.0,0.0,0.0 +0,1.510400,0.0,0.0,0.0,0.0,0.0 +0,1.510500,0.0,0.0,0.0,0.0,0.0 +0,1.510600,0.0,0.0,0.0,0.0,0.0 +0,1.510700,0.0,0.0,0.0,0.0,0.0 +0,1.510800,0.0,0.0,0.0,0.0,0.0 +0,1.510900,0.0,0.0,0.0,0.0,0.0 +0,1.511000,0.0,0.0,0.0,0.0,0.0 +0,1.511100,0.0,0.0,0.0,0.0,0.0 +0,1.511200,0.0,0.0,0.0,0.0,0.0 +0,1.511300,0.0,0.0,0.0,0.0,0.0 +0,1.511400,0.0,0.0,0.0,0.0,0.0 +0,1.511500,0.0,0.0,0.0,0.0,0.0 +0,1.511600,0.0,0.0,0.0,0.0,0.0 +0,1.511700,0.0,0.0,0.0,0.0,0.0 +0,1.511800,0.0,0.0,0.0,0.0,0.0 +0,1.511900,0.0,0.0,0.0,0.0,0.0 +0,1.512000,0.0,0.0,0.0,0.0,0.0 +0,1.512100,0.0,0.0,0.0,0.0,0.0 +0,1.512200,0.0,0.0,0.0,0.0,0.0 +0,1.512300,0.0,0.0,0.0,0.0,0.0 +0,1.512400,0.0,0.0,0.0,0.0,0.0 +0,1.512500,0.0,0.0,0.0,0.0,0.0 +0,1.512600,0.0,0.0,0.0,0.0,0.0 +0,1.512700,0.0,0.0,0.0,0.0,0.0 +0,1.512800,0.0,0.0,0.0,0.0,0.0 +0,1.512900,0.0,0.0,0.0,0.0,0.0 +0,1.513000,0.0,0.0,0.0,0.0,0.0 +0,1.513100,0.0,0.0,0.0,0.0,0.0 +0,1.513200,0.0,0.0,0.0,0.0,0.0 +0,1.513300,0.0,0.0,0.0,0.0,0.0 +0,1.513400,0.0,0.0,0.0,0.0,0.0 +0,1.513500,0.0,0.0,0.0,0.0,0.0 +0,1.513600,0.0,0.0,0.0,0.0,0.0 +0,1.513700,0.0,0.0,0.0,0.0,0.0 +0,1.513800,0.0,0.0,0.0,0.0,0.0 +0,1.513900,0.0,0.0,0.0,0.0,0.0 +0,1.514000,0.0,0.0,0.0,0.0,0.0 +0,1.514100,0.0,0.0,0.0,0.0,0.0 +0,1.514200,0.0,0.0,0.0,0.0,0.0 +0,1.514300,0.0,0.0,0.0,0.0,0.0 +0,1.514400,0.0,0.0,0.0,0.0,0.0 +0,1.514500,0.0,0.0,0.0,0.0,0.0 +0,1.514600,0.0,0.0,0.0,0.0,0.0 +0,1.514700,0.0,0.0,0.0,0.0,0.0 +0,1.514800,0.0,0.0,0.0,0.0,0.0 +0,1.514900,0.0,0.0,0.0,0.0,0.0 +0,1.515000,0.0,0.0,0.0,0.0,0.0 +0,1.515100,0.0,0.0,0.0,0.0,0.0 +0,1.515200,0.0,0.0,0.0,0.0,0.0 +0,1.515300,0.0,0.0,0.0,0.0,0.0 +0,1.515400,0.0,0.0,0.0,0.0,0.0 +0,1.515500,0.0,0.0,0.0,0.0,0.0 +0,1.515600,0.0,0.0,0.0,0.0,0.0 +0,1.515700,0.0,0.0,0.0,0.0,0.0 +0,1.515800,0.0,0.0,0.0,0.0,0.0 +0,1.515900,0.0,0.0,0.0,0.0,0.0 +0,1.516000,0.0,0.0,0.0,0.0,0.0 +0,1.516100,0.0,0.0,0.0,0.0,0.0 +0,1.516200,0.0,0.0,0.0,0.0,0.0 +0,1.516300,0.0,0.0,0.0,0.0,0.0 +0,1.516400,0.0,0.0,0.0,0.0,0.0 +0,1.516500,0.0,0.0,0.0,0.0,0.0 +0,1.516600,0.0,0.0,0.0,0.0,0.0 +0,1.516700,0.0,0.0,0.0,0.0,0.0 +0,1.516800,0.0,0.0,0.0,0.0,0.0 +0,1.516900,0.0,0.0,0.0,0.0,0.0 +0,1.517000,0.0,0.0,0.0,0.0,0.0 +0,1.517100,0.0,0.0,0.0,0.0,0.0 +0,1.517200,0.0,0.0,0.0,0.0,0.0 +0,1.517300,0.0,0.0,0.0,0.0,0.0 +0,1.517400,0.0,0.0,0.0,0.0,0.0 +0,1.517500,0.0,0.0,0.0,0.0,0.0 +0,1.517600,0.0,0.0,0.0,0.0,0.0 +0,1.517700,0.0,0.0,0.0,0.0,0.0 +0,1.517800,0.0,0.0,0.0,0.0,0.0 +0,1.517900,0.0,0.0,0.0,0.0,0.0 +0,1.518000,0.0,0.0,0.0,0.0,0.0 +0,1.518100,0.0,0.0,0.0,0.0,0.0 +0,1.518200,0.0,0.0,0.0,0.0,0.0 +0,1.518300,0.0,0.0,0.0,0.0,0.0 +0,1.518400,0.0,0.0,0.0,0.0,0.0 +0,1.518500,0.0,0.0,0.0,0.0,0.0 +0,1.518600,0.0,0.0,0.0,0.0,0.0 +0,1.518700,0.0,0.0,0.0,0.0,0.0 +0,1.518800,0.0,0.0,0.0,0.0,0.0 +0,1.518900,0.0,0.0,0.0,0.0,0.0 +0,1.519000,0.0,0.0,0.0,0.0,0.0 +0,1.519100,0.0,0.0,0.0,0.0,0.0 +0,1.519200,0.0,0.0,0.0,0.0,0.0 +0,1.519300,0.0,0.0,0.0,0.0,0.0 +0,1.519400,0.0,0.0,0.0,0.0,0.0 +0,1.519500,0.0,0.0,0.0,0.0,0.0 +0,1.519600,0.0,0.0,0.0,0.0,0.0 +0,1.519700,0.0,0.0,0.0,0.0,0.0 +0,1.519800,0.0,0.0,0.0,0.0,0.0 +0,1.519900,0.0,0.0,0.0,0.0,0.0 +0,1.520000,0.0,0.0,0.0,0.0,0.0 +0,1.520100,0.0,0.0,0.0,0.0,0.0 +1,1463.686575,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.520200,0.0,0.0,0.0,0.0,0.0 +0,1.520300,0.0,0.0,0.0,0.0,0.0 +0,1.520400,0.0,0.0,0.0,0.0,0.0 +0,1.520500,0.0,0.0,0.0,0.0,0.0 +0,1.520600,0.0,0.0,0.0,0.0,0.0 +0,1.520700,0.0,0.0,0.0,0.0,0.0 +0,1.520800,0.0,0.0,0.0,0.0,0.0 +0,1.520900,0.0,0.0,0.0,0.0,0.0 +0,1.521000,0.0,0.0,0.0,0.0,0.0 +0,1.521100,0.0,0.0,0.0,0.0,0.0 +0,1.521200,0.0,0.0,0.0,0.0,0.0 +0,1.521300,0.0,0.0,0.0,0.0,0.0 +0,1.521400,0.0,0.0,0.0,0.0,0.0 +0,1.521500,0.0,0.0,0.0,0.0,0.0 +0,1.521600,0.0,0.0,0.0,0.0,0.0 +0,1.521700,0.0,0.0,0.0,0.0,0.0 +0,1.521800,0.0,0.0,0.0,0.0,0.0 +0,1.521900,0.0,0.0,0.0,0.0,0.0 +0,1.522000,0.0,0.0,0.0,0.0,0.0 +0,1.522100,0.0,0.0,0.0,0.0,0.0 +0,1.522200,0.0,0.0,0.0,0.0,0.0 +0,1.522300,0.0,0.0,0.0,0.0,0.0 +0,1.522400,0.0,0.0,0.0,0.0,0.0 +0,1.522500,0.0,0.0,0.0,0.0,0.0 +0,1.522600,0.0,0.0,0.0,0.0,0.0 +0,1.522700,0.0,0.0,0.0,0.0,0.0 +0,1.522800,0.0,0.0,0.0,0.0,0.0 +0,1.522900,0.0,0.0,0.0,0.0,0.0 +0,1.523000,0.0,0.0,0.0,0.0,0.0 +0,1.523100,0.0,0.0,0.0,0.0,0.0 +0,1.523200,0.0,0.0,0.0,0.0,0.0 +0,1.523300,0.0,0.0,0.0,0.0,0.0 +0,1.523400,0.0,0.0,0.0,0.0,0.0 +0,1.523500,0.0,0.0,0.0,0.0,0.0 +0,1.523600,0.0,0.0,0.0,0.0,0.0 +0,1.523700,0.0,0.0,0.0,0.0,0.0 +0,1.523800,0.0,0.0,0.0,0.0,0.0 +0,1.523900,0.0,0.0,0.0,0.0,0.0 +0,1.524000,0.0,0.0,0.0,0.0,0.0 +0,1.524100,0.0,0.0,0.0,0.0,0.0 +0,1.524200,0.0,0.0,0.0,0.0,0.0 +0,1.524300,0.0,0.0,0.0,0.0,0.0 +0,1.524400,0.0,0.0,0.0,0.0,0.0 +0,1.524500,0.0,0.0,0.0,0.0,0.0 +0,1.524600,0.0,0.0,0.0,0.0,0.0 +0,1.524700,0.0,0.0,0.0,0.0,0.0 +0,1.524800,0.0,0.0,0.0,0.0,0.0 +0,1.524900,0.0,0.0,0.0,0.0,0.0 +0,1.525000,0.0,0.0,0.0,0.0,0.0 +0,1.525100,0.0,0.0,0.0,0.0,0.0 +0,1.525200,0.0,0.0,0.0,0.0,0.0 +0,1.525300,0.0,0.0,0.0,0.0,0.0 +0,1.525400,0.0,0.0,0.0,0.0,0.0 +0,1.525500,0.0,0.0,0.0,0.0,0.0 +0,1.525600,0.0,0.0,0.0,0.0,0.0 +0,1.525700,0.0,0.0,0.0,0.0,0.0 +0,1.525800,0.0,0.0,0.0,0.0,0.0 +0,1.525900,0.0,0.0,0.0,0.0,0.0 +0,1.526000,0.0,0.0,0.0,0.0,0.0 +0,1.526100,0.0,0.0,0.0,0.0,0.0 +0,1.526200,0.0,0.0,0.0,0.0,0.0 +0,1.526300,0.0,0.0,0.0,0.0,0.0 +0,1.526400,0.0,0.0,0.0,0.0,0.0 +0,1.526500,0.0,0.0,0.0,0.0,0.0 +0,1.526600,0.0,0.0,0.0,0.0,0.0 +0,1.526700,0.0,0.0,0.0,0.0,0.0 +0,1.526800,0.0,0.0,0.0,0.0,0.0 +0,1.526900,0.0,0.0,0.0,0.0,0.0 +0,1.527000,0.0,0.0,0.0,0.0,0.0 +0,1.527100,0.0,0.0,0.0,0.0,0.0 +0,1.527200,0.0,0.0,0.0,0.0,0.0 +0,1.527300,0.0,0.0,0.0,0.0,0.0 +0,1.527400,0.0,0.0,0.0,0.0,0.0 +0,1.527500,0.0,0.0,0.0,0.0,0.0 +0,1.527600,0.0,0.0,0.0,0.0,0.0 +0,1.527700,0.0,0.0,0.0,0.0,0.0 +0,1.527800,0.0,0.0,0.0,0.0,0.0 +0,1.527900,0.0,0.0,0.0,0.0,0.0 +0,1.528000,0.0,0.0,0.0,0.0,0.0 +0,1.528100,0.0,0.0,0.0,0.0,0.0 +0,1.528200,0.0,0.0,0.0,0.0,0.0 +0,1.528300,0.0,0.0,0.0,0.0,0.0 +0,1.528400,0.0,0.0,0.0,0.0,0.0 +0,1.528500,0.0,0.0,0.0,0.0,0.0 +0,1.528600,0.0,0.0,0.0,0.0,0.0 +0,1.528700,0.0,0.0,0.0,0.0,0.0 +0,1.528800,0.0,0.0,0.0,0.0,0.0 +0,1.528900,0.0,0.0,0.0,0.0,0.0 +0,1.529000,0.0,0.0,0.0,0.0,0.0 +0,1.529100,0.0,0.0,0.0,0.0,0.0 +0,1.529200,0.0,0.0,0.0,0.0,0.0 +0,1.529300,0.0,0.0,0.0,0.0,0.0 +0,1.529400,0.0,0.0,0.0,0.0,0.0 +0,1.529500,0.0,0.0,0.0,0.0,0.0 +0,1.529600,0.0,0.0,0.0,0.0,0.0 +0,1.529700,0.0,0.0,0.0,0.0,0.0 +0,1.529800,0.0,0.0,0.0,0.0,0.0 +0,1.529900,0.0,0.0,0.0,0.0,0.0 +0,1.530000,0.0,0.0,0.0,0.0,0.0 +0,1.530100,0.0,0.0,0.0,0.0,0.0 +0,1.530200,0.0,0.0,0.0,0.0,0.0 +0,1.530300,0.0,0.0,0.0,0.0,0.0 +0,1.530400,0.0,0.0,0.0,0.0,0.0 +0,1.530500,0.0,0.0,0.0,0.0,0.0 +0,1.530600,0.0,0.0,0.0,0.0,0.0 +0,1.530700,0.0,0.0,0.0,0.0,0.0 +0,1.530800,0.0,0.0,0.0,0.0,0.0 +0,1.530900,0.0,0.0,0.0,0.0,0.0 +0,1.531000,0.0,0.0,0.0,0.0,0.0 +0,1.531100,0.0,0.0,0.0,0.0,0.0 +0,1.531200,0.0,0.0,0.0,0.0,0.0 +0,1.531300,0.0,0.0,0.0,0.0,0.0 +0,1.531400,0.0,0.0,0.0,0.0,0.0 +0,1.531500,0.0,0.0,0.0,0.0,0.0 +0,1.531600,0.0,0.0,0.0,0.0,0.0 +0,1.531700,0.0,0.0,0.0,0.0,0.0 +0,1.531800,0.0,0.0,0.0,0.0,0.0 +0,1.531900,0.0,0.0,0.0,0.0,0.0 +0,1.532000,0.0,0.0,0.0,0.0,0.0 +0,1.532100,0.0,0.0,0.0,0.0,0.0 +0,1.532200,0.0,0.0,0.0,0.0,0.0 +0,1.532300,0.0,0.0,0.0,0.0,0.0 +0,1.532400,0.0,0.0,0.0,0.0,0.0 +0,1.532500,0.0,0.0,0.0,0.0,0.0 +0,1.532600,0.0,0.0,0.0,0.0,0.0 +0,1.532700,0.0,0.0,0.0,0.0,0.0 +0,1.532800,0.0,0.0,0.0,0.0,0.0 +0,1.532900,0.0,0.0,0.0,0.0,0.0 +0,1.533000,0.0,0.0,0.0,0.0,0.0 +0,1.533100,0.0,0.0,0.0,0.0,0.0 +0,1.533200,0.0,0.0,0.0,0.0,0.0 +0,1.533300,0.0,0.0,0.0,0.0,0.0 +0,1.533400,0.0,0.0,0.0,0.0,0.0 +0,1.533500,0.0,0.0,0.0,0.0,0.0 +0,1.533600,0.0,0.0,0.0,0.0,0.0 +0,1.533700,0.0,0.0,0.0,0.0,0.0 +0,1.533800,0.0,0.0,0.0,0.0,0.0 +0,1.533900,0.0,0.0,0.0,0.0,0.0 +0,1.534000,0.0,0.0,0.0,0.0,0.0 +0,1.534100,0.0,0.0,0.0,0.0,0.0 +0,1.534200,0.0,0.0,0.0,0.0,0.0 +0,1.534300,0.0,0.0,0.0,0.0,0.0 +0,1.534400,0.0,0.0,0.0,0.0,0.0 +0,1.534500,0.0,0.0,0.0,0.0,0.0 +0,1.534600,0.0,0.0,0.0,0.0,0.0 +0,1.534700,0.0,0.0,0.0,0.0,0.0 +0,1.534800,0.0,0.0,0.0,0.0,0.0 +0,1.534900,0.0,0.0,0.0,0.0,0.0 +0,1.535000,0.0,0.0,0.0,0.0,0.0 +0,1.535100,0.0,0.0,0.0,0.0,0.0 +0,1.535200,0.0,0.0,0.0,0.0,0.0 +0,1.535300,0.0,0.0,0.0,0.0,0.0 +0,1.535400,0.0,0.0,0.0,0.0,0.0 +0,1.535500,0.0,0.0,0.0,0.0,0.0 +0,1.535600,0.0,0.0,0.0,0.0,0.0 +0,1.535700,0.0,0.0,0.0,0.0,0.0 +0,1.535800,0.0,0.0,0.0,0.0,0.0 +0,1.535900,0.0,0.0,0.0,0.0,0.0 +0,1.536000,0.0,0.0,0.0,0.0,0.0 +0,1.536100,0.0,0.0,0.0,0.0,0.0 +0,1.536200,0.0,0.0,0.0,0.0,0.0 +0,1.536300,0.0,0.0,0.0,0.0,0.0 +0,1.536400,0.0,0.0,0.0,0.0,0.0 +0,1.536500,0.0,0.0,0.0,0.0,0.0 +0,1.536600,0.0,0.0,0.0,0.0,0.0 +0,1.536700,0.0,0.0,0.0,0.0,0.0 +0,1.536800,0.0,0.0,0.0,0.0,0.0 +0,1.536900,0.0,0.0,0.0,0.0,0.0 +0,1.537000,0.0,0.0,0.0,0.0,0.0 +0,1.537100,0.0,0.0,0.0,0.0,0.0 +0,1.537200,0.0,0.0,0.0,0.0,0.0 +0,1.537300,0.0,0.0,0.0,0.0,0.0 +0,1.537400,0.0,0.0,0.0,0.0,0.0 +0,1.537500,0.0,0.0,0.0,0.0,0.0 +0,1.537600,0.0,0.0,0.0,0.0,0.0 +0,1.537700,0.0,0.0,0.0,0.0,0.0 +0,1.537800,0.0,0.0,0.0,0.0,0.0 +0,1.537900,0.0,0.0,0.0,0.0,0.0 +0,1.538000,0.0,0.0,0.0,0.0,0.0 +0,1.538100,0.0,0.0,0.0,0.0,0.0 +0,1.538200,0.0,0.0,0.0,0.0,0.0 +0,1.538300,0.0,0.0,0.0,0.0,0.0 +0,1.538400,0.0,0.0,0.0,0.0,0.0 +0,1.538500,0.0,0.0,0.0,0.0,0.0 +0,1.538600,0.0,0.0,0.0,0.0,0.0 +0,1.538700,0.0,0.0,0.0,0.0,0.0 +0,1.538800,0.0,0.0,0.0,0.0,0.0 +0,1.538900,0.0,0.0,0.0,0.0,0.0 +0,1.539000,0.0,0.0,0.0,0.0,0.0 +0,1.539100,0.0,0.0,0.0,0.0,0.0 +0,1.539200,0.0,0.0,0.0,0.0,0.0 +0,1.539300,0.0,0.0,0.0,0.0,0.0 +0,1.539400,0.0,0.0,0.0,0.0,0.0 +0,1.539500,0.0,0.0,0.0,0.0,0.0 +0,1.539600,0.0,0.0,0.0,0.0,0.0 +0,1.539700,0.0,0.0,0.0,0.0,0.0 +0,1.539800,0.0,0.0,0.0,0.0,0.0 +0,1.539900,0.0,0.0,0.0,0.0,0.0 +0,1.540000,0.0,0.0,0.0,0.0,0.0 +0,1.540100,0.0,0.0,0.0,0.0,0.0 +1,1522.221383,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.540200,0.0,0.0,0.0,0.0,0.0 +0,1.540300,0.0,0.0,0.0,0.0,0.0 +0,1.540400,0.0,0.0,0.0,0.0,0.0 +0,1.540500,0.0,0.0,0.0,0.0,0.0 +0,1.540600,0.0,0.0,0.0,0.0,0.0 +0,1.540700,0.0,0.0,0.0,0.0,0.0 +0,1.540800,0.0,0.0,0.0,0.0,0.0 +0,1.540900,0.0,0.0,0.0,0.0,0.0 +0,1.541000,0.0,0.0,0.0,0.0,0.0 +0,1.541100,0.0,0.0,0.0,0.0,0.0 +0,1.541200,0.0,0.0,0.0,0.0,0.0 +0,1.541300,0.0,0.0,0.0,0.0,0.0 +0,1.541400,0.0,0.0,0.0,0.0,0.0 +0,1.541500,0.0,0.0,0.0,0.0,0.0 +0,1.541600,0.0,0.0,0.0,0.0,0.0 +0,1.541700,0.0,0.0,0.0,0.0,0.0 +0,1.541800,0.0,0.0,0.0,0.0,0.0 +0,1.541900,0.0,0.0,0.0,0.0,0.0 +0,1.542000,0.0,0.0,0.0,0.0,0.0 +0,1.542100,0.0,0.0,0.0,0.0,0.0 +0,1.542200,0.0,0.0,0.0,0.0,0.0 +0,1.542300,0.0,0.0,0.0,0.0,0.0 +0,1.542400,0.0,0.0,0.0,0.0,0.0 +0,1.542500,0.0,0.0,0.0,0.0,0.0 +0,1.542600,0.0,0.0,0.0,0.0,0.0 +0,1.542700,0.0,0.0,0.0,0.0,0.0 +0,1.542800,0.0,0.0,0.0,0.0,0.0 +0,1.542900,0.0,0.0,0.0,0.0,0.0 +0,1.543000,0.0,0.0,0.0,0.0,0.0 +0,1.543100,0.0,0.0,0.0,0.0,0.0 +0,1.543200,0.0,0.0,0.0,0.0,0.0 +0,1.543300,0.0,0.0,0.0,0.0,0.0 +0,1.543400,0.0,0.0,0.0,0.0,0.0 +0,1.543500,0.0,0.0,0.0,0.0,0.0 +0,1.543600,0.0,0.0,0.0,0.0,0.0 +0,1.543700,0.0,0.0,0.0,0.0,0.0 +0,1.543800,0.0,0.0,0.0,0.0,0.0 +0,1.543900,0.0,0.0,0.0,0.0,0.0 +0,1.544000,0.0,0.0,0.0,0.0,0.0 +0,1.544100,0.0,0.0,0.0,0.0,0.0 +0,1.544200,0.0,0.0,0.0,0.0,0.0 +0,1.544300,0.0,0.0,0.0,0.0,0.0 +0,1.544400,0.0,0.0,0.0,0.0,0.0 +0,1.544500,0.0,0.0,0.0,0.0,0.0 +0,1.544600,0.0,0.0,0.0,0.0,0.0 +0,1.544700,0.0,0.0,0.0,0.0,0.0 +0,1.544800,0.0,0.0,0.0,0.0,0.0 +0,1.544900,0.0,0.0,0.0,0.0,0.0 +0,1.545000,0.0,0.0,0.0,0.0,0.0 +0,1.545100,0.0,0.0,0.0,0.0,0.0 +0,1.545200,0.0,0.0,0.0,0.0,0.0 +0,1.545300,0.0,0.0,0.0,0.0,0.0 +0,1.545400,0.0,0.0,0.0,0.0,0.0 +0,1.545500,0.0,0.0,0.0,0.0,0.0 +0,1.545600,0.0,0.0,0.0,0.0,0.0 +0,1.545700,0.0,0.0,0.0,0.0,0.0 +0,1.545800,0.0,0.0,0.0,0.0,0.0 +0,1.545900,0.0,0.0,0.0,0.0,0.0 +0,1.546000,0.0,0.0,0.0,0.0,0.0 +0,1.546100,0.0,0.0,0.0,0.0,0.0 +0,1.546200,0.0,0.0,0.0,0.0,0.0 +0,1.546300,0.0,0.0,0.0,0.0,0.0 +0,1.546400,0.0,0.0,0.0,0.0,0.0 +0,1.546500,0.0,0.0,0.0,0.0,0.0 +0,1.546600,0.0,0.0,0.0,0.0,0.0 +0,1.546700,0.0,0.0,0.0,0.0,0.0 +0,1.546800,0.0,0.0,0.0,0.0,0.0 +0,1.546900,0.0,0.0,0.0,0.0,0.0 +0,1.547000,0.0,0.0,0.0,0.0,0.0 +0,1.547100,0.0,0.0,0.0,0.0,0.0 +0,1.547200,0.0,0.0,0.0,0.0,0.0 +0,1.547300,0.0,0.0,0.0,0.0,0.0 +0,1.547400,0.0,0.0,0.0,0.0,0.0 +0,1.547500,0.0,0.0,0.0,0.0,0.0 +0,1.547600,0.0,0.0,0.0,0.0,0.0 +0,1.547700,0.0,0.0,0.0,0.0,0.0 +0,1.547800,0.0,0.0,0.0,0.0,0.0 +0,1.547900,0.0,0.0,0.0,0.0,0.0 +0,1.548000,0.0,0.0,0.0,0.0,0.0 +0,1.548100,0.0,0.0,0.0,0.0,0.0 +0,1.548200,0.0,0.0,0.0,0.0,0.0 +0,1.548300,0.0,0.0,0.0,0.0,0.0 +0,1.548400,0.0,0.0,0.0,0.0,0.0 +0,1.548500,0.0,0.0,0.0,0.0,0.0 +0,1.548600,0.0,0.0,0.0,0.0,0.0 +0,1.548700,0.0,0.0,0.0,0.0,0.0 +0,1.548800,0.0,0.0,0.0,0.0,0.0 +0,1.548900,0.0,0.0,0.0,0.0,0.0 +0,1.549000,0.0,0.0,0.0,0.0,0.0 +0,1.549100,0.0,0.0,0.0,0.0,0.0 +0,1.549200,0.0,0.0,0.0,0.0,0.0 +0,1.549300,0.0,0.0,0.0,0.0,0.0 +0,1.549400,0.0,0.0,0.0,0.0,0.0 +0,1.549500,0.0,0.0,0.0,0.0,0.0 +0,1.549600,0.0,0.0,0.0,0.0,0.0 +0,1.549700,0.0,0.0,0.0,0.0,0.0 +0,1.549800,0.0,0.0,0.0,0.0,0.0 +0,1.549900,0.0,0.0,0.0,0.0,0.0 +0,1.550000,0.0,0.0,0.0,0.0,0.0 +0,1.550100,0.0,0.0,0.0,0.0,0.0 +0,1.550200,0.0,0.0,0.0,0.0,0.0 +0,1.550300,0.0,0.0,0.0,0.0,0.0 +0,1.550400,0.0,0.0,0.0,0.0,0.0 +0,1.550500,0.0,0.0,0.0,0.0,0.0 +0,1.550600,0.0,0.0,0.0,0.0,0.0 +0,1.550700,0.0,0.0,0.0,0.0,0.0 +0,1.550800,0.0,0.0,0.0,0.0,0.0 +0,1.550900,0.0,0.0,0.0,0.0,0.0 +0,1.551000,0.0,0.0,0.0,0.0,0.0 +0,1.551100,0.0,0.0,0.0,0.0,0.0 +0,1.551200,0.0,0.0,0.0,0.0,0.0 +0,1.551300,0.0,0.0,0.0,0.0,0.0 +0,1.551400,0.0,0.0,0.0,0.0,0.0 +0,1.551500,0.0,0.0,0.0,0.0,0.0 +0,1.551600,0.0,0.0,0.0,0.0,0.0 +0,1.551700,0.0,0.0,0.0,0.0,0.0 +0,1.551800,0.0,0.0,0.0,0.0,0.0 +0,1.551900,0.0,0.0,0.0,0.0,0.0 +0,1.552000,0.0,0.0,0.0,0.0,0.0 +0,1.552100,0.0,0.0,0.0,0.0,0.0 +0,1.552200,0.0,0.0,0.0,0.0,0.0 +0,1.552300,0.0,0.0,0.0,0.0,0.0 +0,1.552400,0.0,0.0,0.0,0.0,0.0 +0,1.552500,0.0,0.0,0.0,0.0,0.0 +0,1.552600,0.0,0.0,0.0,0.0,0.0 +0,1.552700,0.0,0.0,0.0,0.0,0.0 +0,1.552800,0.0,0.0,0.0,0.0,0.0 +0,1.552900,0.0,0.0,0.0,0.0,0.0 +0,1.553000,0.0,0.0,0.0,0.0,0.0 +0,1.553100,0.0,0.0,0.0,0.0,0.0 +0,1.553200,0.0,0.0,0.0,0.0,0.0 +0,1.553300,0.0,0.0,0.0,0.0,0.0 +0,1.553400,0.0,0.0,0.0,0.0,0.0 +0,1.553500,0.0,0.0,0.0,0.0,0.0 +0,1.553600,0.0,0.0,0.0,0.0,0.0 +0,1.553700,0.0,0.0,0.0,0.0,0.0 +0,1.553800,0.0,0.0,0.0,0.0,0.0 +0,1.553900,0.0,0.0,0.0,0.0,0.0 +0,1.554000,0.0,0.0,0.0,0.0,0.0 +0,1.554100,0.0,0.0,0.0,0.0,0.0 +0,1.554200,0.0,0.0,0.0,0.0,0.0 +0,1.554300,0.0,0.0,0.0,0.0,0.0 +0,1.554400,0.0,0.0,0.0,0.0,0.0 +0,1.554500,0.0,0.0,0.0,0.0,0.0 +0,1.554600,0.0,0.0,0.0,0.0,0.0 +0,1.554700,0.0,0.0,0.0,0.0,0.0 +0,1.554800,0.0,0.0,0.0,0.0,0.0 +0,1.554900,0.0,0.0,0.0,0.0,0.0 +0,1.555000,0.0,0.0,0.0,0.0,0.0 +0,1.555100,0.0,0.0,0.0,0.0,0.0 +0,1.555200,0.0,0.0,0.0,0.0,0.0 +0,1.555300,0.0,0.0,0.0,0.0,0.0 +0,1.555400,0.0,0.0,0.0,0.0,0.0 +0,1.555500,0.0,0.0,0.0,0.0,0.0 +0,1.555600,0.0,0.0,0.0,0.0,0.0 +0,1.555700,0.0,0.0,0.0,0.0,0.0 +0,1.555800,0.0,0.0,0.0,0.0,0.0 +0,1.555900,0.0,0.0,0.0,0.0,0.0 +0,1.556000,0.0,0.0,0.0,0.0,0.0 +0,1.556100,0.0,0.0,0.0,0.0,0.0 +0,1.556200,0.0,0.0,0.0,0.0,0.0 +0,1.556300,0.0,0.0,0.0,0.0,0.0 +0,1.556400,0.0,0.0,0.0,0.0,0.0 +0,1.556500,0.0,0.0,0.0,0.0,0.0 +0,1.556600,0.0,0.0,0.0,0.0,0.0 +0,1.556700,0.0,0.0,0.0,0.0,0.0 +0,1.556800,0.0,0.0,0.0,0.0,0.0 +0,1.556900,0.0,0.0,0.0,0.0,0.0 +0,1.557000,0.0,0.0,0.0,0.0,0.0 +0,1.557100,0.0,0.0,0.0,0.0,0.0 +0,1.557200,0.0,0.0,0.0,0.0,0.0 +0,1.557300,0.0,0.0,0.0,0.0,0.0 +0,1.557400,0.0,0.0,0.0,0.0,0.0 +0,1.557500,0.0,0.0,0.0,0.0,0.0 +0,1.557600,0.0,0.0,0.0,0.0,0.0 +0,1.557700,0.0,0.0,0.0,0.0,0.0 +0,1.557800,0.0,0.0,0.0,0.0,0.0 +0,1.557900,0.0,0.0,0.0,0.0,0.0 +0,1.558000,0.0,0.0,0.0,0.0,0.0 +0,1.558100,0.0,0.0,0.0,0.0,0.0 +0,1.558200,0.0,0.0,0.0,0.0,0.0 +0,1.558300,0.0,0.0,0.0,0.0,0.0 +0,1.558400,0.0,0.0,0.0,0.0,0.0 +0,1.558500,0.0,0.0,0.0,0.0,0.0 +0,1.558600,0.0,0.0,0.0,0.0,0.0 +0,1.558700,0.0,0.0,0.0,0.0,0.0 +0,1.558800,0.0,0.0,0.0,0.0,0.0 +0,1.558900,0.0,0.0,0.0,0.0,0.0 +0,1.559000,0.0,0.0,0.0,0.0,0.0 +0,1.559100,0.0,0.0,0.0,0.0,0.0 +0,1.559200,0.0,0.0,0.0,0.0,0.0 +0,1.559300,0.0,0.0,0.0,0.0,0.0 +0,1.559400,0.0,0.0,0.0,0.0,0.0 +0,1.559500,0.0,0.0,0.0,0.0,0.0 +0,1.559600,0.0,0.0,0.0,0.0,0.0 +0,1.559700,0.0,0.0,0.0,0.0,0.0 +0,1.559800,0.0,0.0,0.0,0.0,0.0 +0,1.559900,0.0,0.0,0.0,0.0,0.0 +0,1.560000,0.0,0.0,0.0,0.0,0.0 +0,1.560100,0.0,0.0,0.0,0.0,0.0 +1,1582.296342,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.560200,0.0,0.0,0.0,0.0,0.0 +0,1.560300,0.0,0.0,0.0,0.0,0.0 +0,1.560400,0.0,0.0,0.0,0.0,0.0 +0,1.560500,0.0,0.0,0.0,0.0,0.0 +0,1.560600,0.0,0.0,0.0,0.0,0.0 +0,1.560700,0.0,0.0,0.0,0.0,0.0 +0,1.560800,0.0,0.0,0.0,0.0,0.0 +0,1.560900,0.0,0.0,0.0,0.0,0.0 +0,1.561000,0.0,0.0,0.0,0.0,0.0 +0,1.561100,0.0,0.0,0.0,0.0,0.0 +0,1.561200,0.0,0.0,0.0,0.0,0.0 +0,1.561300,0.0,0.0,0.0,0.0,0.0 +0,1.561400,0.0,0.0,0.0,0.0,0.0 +0,1.561500,0.0,0.0,0.0,0.0,0.0 +0,1.561600,0.0,0.0,0.0,0.0,0.0 +0,1.561700,0.0,0.0,0.0,0.0,0.0 +0,1.561800,0.0,0.0,0.0,0.0,0.0 +0,1.561900,0.0,0.0,0.0,0.0,0.0 +0,1.562000,0.0,0.0,0.0,0.0,0.0 +0,1.562100,0.0,0.0,0.0,0.0,0.0 +0,1.562200,0.0,0.0,0.0,0.0,0.0 +0,1.562300,0.0,0.0,0.0,0.0,0.0 +0,1.562400,0.0,0.0,0.0,0.0,0.0 +0,1.562500,0.0,0.0,0.0,0.0,0.0 +0,1.562600,0.0,0.0,0.0,0.0,0.0 +0,1.562700,0.0,0.0,0.0,0.0,0.0 +0,1.562800,0.0,0.0,0.0,0.0,0.0 +0,1.562900,0.0,0.0,0.0,0.0,0.0 +0,1.563000,0.0,0.0,0.0,0.0,0.0 +0,1.563100,0.0,0.0,0.0,0.0,0.0 +0,1.563200,0.0,0.0,0.0,0.0,0.0 +0,1.563300,0.0,0.0,0.0,0.0,0.0 +0,1.563400,0.0,0.0,0.0,0.0,0.0 +0,1.563500,0.0,0.0,0.0,0.0,0.0 +0,1.563600,0.0,0.0,0.0,0.0,0.0 +0,1.563700,0.0,0.0,0.0,0.0,0.0 +0,1.563800,0.0,0.0,0.0,0.0,0.0 +0,1.563900,0.0,0.0,0.0,0.0,0.0 +0,1.564000,0.0,0.0,0.0,0.0,0.0 +0,1.564100,0.0,0.0,0.0,0.0,0.0 +0,1.564200,0.0,0.0,0.0,0.0,0.0 +0,1.564300,0.0,0.0,0.0,0.0,0.0 +0,1.564400,0.0,0.0,0.0,0.0,0.0 +0,1.564500,0.0,0.0,0.0,0.0,0.0 +0,1.564600,0.0,0.0,0.0,0.0,0.0 +0,1.564700,0.0,0.0,0.0,0.0,0.0 +0,1.564800,0.0,0.0,0.0,0.0,0.0 +0,1.564900,0.0,0.0,0.0,0.0,0.0 +0,1.565000,0.0,0.0,0.0,0.0,0.0 +0,1.565100,0.0,0.0,0.0,0.0,0.0 +0,1.565200,0.0,0.0,0.0,0.0,0.0 +0,1.565300,0.0,0.0,0.0,0.0,0.0 +0,1.565400,0.0,0.0,0.0,0.0,0.0 +0,1.565500,0.0,0.0,0.0,0.0,0.0 +0,1.565600,0.0,0.0,0.0,0.0,0.0 +0,1.565700,0.0,0.0,0.0,0.0,0.0 +0,1.565800,0.0,0.0,0.0,0.0,0.0 +0,1.565900,0.0,0.0,0.0,0.0,0.0 +0,1.566000,0.0,0.0,0.0,0.0,0.0 +0,1.566100,0.0,0.0,0.0,0.0,0.0 +0,1.566200,0.0,0.0,0.0,0.0,0.0 +0,1.566300,0.0,0.0,0.0,0.0,0.0 +0,1.566400,0.0,0.0,0.0,0.0,0.0 +0,1.566500,0.0,0.0,0.0,0.0,0.0 +0,1.566600,0.0,0.0,0.0,0.0,0.0 +0,1.566700,0.0,0.0,0.0,0.0,0.0 +0,1.566800,0.0,0.0,0.0,0.0,0.0 +0,1.566900,0.0,0.0,0.0,0.0,0.0 +0,1.567000,0.0,0.0,0.0,0.0,0.0 +0,1.567100,0.0,0.0,0.0,0.0,0.0 +0,1.567200,0.0,0.0,0.0,0.0,0.0 +0,1.567300,0.0,0.0,0.0,0.0,0.0 +0,1.567400,0.0,0.0,0.0,0.0,0.0 +0,1.567500,0.0,0.0,0.0,0.0,0.0 +0,1.567600,0.0,0.0,0.0,0.0,0.0 +0,1.567700,0.0,0.0,0.0,0.0,0.0 +0,1.567800,0.0,0.0,0.0,0.0,0.0 +0,1.567900,0.0,0.0,0.0,0.0,0.0 +0,1.568000,0.0,0.0,0.0,0.0,0.0 +0,1.568100,0.0,0.0,0.0,0.0,0.0 +0,1.568200,0.0,0.0,0.0,0.0,0.0 +0,1.568300,0.0,0.0,0.0,0.0,0.0 +0,1.568400,0.0,0.0,0.0,0.0,0.0 +0,1.568500,0.0,0.0,0.0,0.0,0.0 +0,1.568600,0.0,0.0,0.0,0.0,0.0 +0,1.568700,0.0,0.0,0.0,0.0,0.0 +0,1.568800,0.0,0.0,0.0,0.0,0.0 +0,1.568900,0.0,0.0,0.0,0.0,0.0 +0,1.569000,0.0,0.0,0.0,0.0,0.0 +0,1.569100,0.0,0.0,0.0,0.0,0.0 +0,1.569200,0.0,0.0,0.0,0.0,0.0 +0,1.569300,0.0,0.0,0.0,0.0,0.0 +0,1.569400,0.0,0.0,0.0,0.0,0.0 +0,1.569500,0.0,0.0,0.0,0.0,0.0 +0,1.569600,0.0,0.0,0.0,0.0,0.0 +0,1.569700,0.0,0.0,0.0,0.0,0.0 +0,1.569800,0.0,0.0,0.0,0.0,0.0 +0,1.569900,0.0,0.0,0.0,0.0,0.0 +0,1.570000,0.0,0.0,0.0,0.0,0.0 +0,1.570100,0.0,0.0,0.0,0.0,0.0 +0,1.570200,0.0,0.0,0.0,0.0,0.0 +0,1.570300,0.0,0.0,0.0,0.0,0.0 +0,1.570400,0.0,0.0,0.0,0.0,0.0 +0,1.570500,0.0,0.0,0.0,0.0,0.0 +0,1.570600,0.0,0.0,0.0,0.0,0.0 +0,1.570700,0.0,0.0,0.0,0.0,0.0 +0,1.570800,0.0,0.0,0.0,0.0,0.0 +0,1.570900,0.0,0.0,0.0,0.0,0.0 +0,1.571000,0.0,0.0,0.0,0.0,0.0 +0,1.571100,0.0,0.0,0.0,0.0,0.0 +0,1.571200,0.0,0.0,0.0,0.0,0.0 +0,1.571300,0.0,0.0,0.0,0.0,0.0 +0,1.571400,0.0,0.0,0.0,0.0,0.0 +0,1.571500,0.0,0.0,0.0,0.0,0.0 +0,1.571600,0.0,0.0,0.0,0.0,0.0 +0,1.571700,0.0,0.0,0.0,0.0,0.0 +0,1.571800,0.0,0.0,0.0,0.0,0.0 +0,1.571900,0.0,0.0,0.0,0.0,0.0 +0,1.572000,0.0,0.0,0.0,0.0,0.0 +0,1.572100,0.0,0.0,0.0,0.0,0.0 +0,1.572200,0.0,0.0,0.0,0.0,0.0 +0,1.572300,0.0,0.0,0.0,0.0,0.0 +0,1.572400,0.0,0.0,0.0,0.0,0.0 +0,1.572500,0.0,0.0,0.0,0.0,0.0 +0,1.572600,0.0,0.0,0.0,0.0,0.0 +0,1.572700,0.0,0.0,0.0,0.0,0.0 +0,1.572800,0.0,0.0,0.0,0.0,0.0 +0,1.572900,0.0,0.0,0.0,0.0,0.0 +0,1.573000,0.0,0.0,0.0,0.0,0.0 +0,1.573100,0.0,0.0,0.0,0.0,0.0 +0,1.573200,0.0,0.0,0.0,0.0,0.0 +0,1.573300,0.0,0.0,0.0,0.0,0.0 +0,1.573400,0.0,0.0,0.0,0.0,0.0 +0,1.573500,0.0,0.0,0.0,0.0,0.0 +0,1.573600,0.0,0.0,0.0,0.0,0.0 +0,1.573700,0.0,0.0,0.0,0.0,0.0 +0,1.573800,0.0,0.0,0.0,0.0,0.0 +0,1.573900,0.0,0.0,0.0,0.0,0.0 +0,1.574000,0.0,0.0,0.0,0.0,0.0 +0,1.574100,0.0,0.0,0.0,0.0,0.0 +0,1.574200,0.0,0.0,0.0,0.0,0.0 +0,1.574300,0.0,0.0,0.0,0.0,0.0 +0,1.574400,0.0,0.0,0.0,0.0,0.0 +0,1.574500,0.0,0.0,0.0,0.0,0.0 +0,1.574600,0.0,0.0,0.0,0.0,0.0 +0,1.574700,0.0,0.0,0.0,0.0,0.0 +0,1.574800,0.0,0.0,0.0,0.0,0.0 +0,1.574900,0.0,0.0,0.0,0.0,0.0 +0,1.575000,0.0,0.0,0.0,0.0,0.0 +0,1.575100,0.0,0.0,0.0,0.0,0.0 +0,1.575200,0.0,0.0,0.0,0.0,0.0 +0,1.575300,0.0,0.0,0.0,0.0,0.0 +0,1.575400,0.0,0.0,0.0,0.0,0.0 +0,1.575500,0.0,0.0,0.0,0.0,0.0 +0,1.575600,0.0,0.0,0.0,0.0,0.0 +0,1.575700,0.0,0.0,0.0,0.0,0.0 +0,1.575800,0.0,0.0,0.0,0.0,0.0 +0,1.575900,0.0,0.0,0.0,0.0,0.0 +0,1.576000,0.0,0.0,0.0,0.0,0.0 +0,1.576100,0.0,0.0,0.0,0.0,0.0 +0,1.576200,0.0,0.0,0.0,0.0,0.0 +0,1.576300,0.0,0.0,0.0,0.0,0.0 +0,1.576400,0.0,0.0,0.0,0.0,0.0 +0,1.576500,0.0,0.0,0.0,0.0,0.0 +0,1.576600,0.0,0.0,0.0,0.0,0.0 +0,1.576700,0.0,0.0,0.0,0.0,0.0 +0,1.576800,0.0,0.0,0.0,0.0,0.0 +0,1.576900,0.0,0.0,0.0,0.0,0.0 +0,1.577000,0.0,0.0,0.0,0.0,0.0 +0,1.577100,0.0,0.0,0.0,0.0,0.0 +0,1.577200,0.0,0.0,0.0,0.0,0.0 +0,1.577300,0.0,0.0,0.0,0.0,0.0 +0,1.577400,0.0,0.0,0.0,0.0,0.0 +0,1.577500,0.0,0.0,0.0,0.0,0.0 +0,1.577600,0.0,0.0,0.0,0.0,0.0 +0,1.577700,0.0,0.0,0.0,0.0,0.0 +0,1.577800,0.0,0.0,0.0,0.0,0.0 +0,1.577900,0.0,0.0,0.0,0.0,0.0 +0,1.578000,0.0,0.0,0.0,0.0,0.0 +0,1.578100,0.0,0.0,0.0,0.0,0.0 +0,1.578200,0.0,0.0,0.0,0.0,0.0 +0,1.578300,0.0,0.0,0.0,0.0,0.0 +0,1.578400,0.0,0.0,0.0,0.0,0.0 +0,1.578500,0.0,0.0,0.0,0.0,0.0 +0,1.578600,0.0,0.0,0.0,0.0,0.0 +0,1.578700,0.0,0.0,0.0,0.0,0.0 +0,1.578800,0.0,0.0,0.0,0.0,0.0 +0,1.578900,0.0,0.0,0.0,0.0,0.0 +0,1.579000,0.0,0.0,0.0,0.0,0.0 +0,1.579100,0.0,0.0,0.0,0.0,0.0 +0,1.579200,0.0,0.0,0.0,0.0,0.0 +0,1.579300,0.0,0.0,0.0,0.0,0.0 +0,1.579400,0.0,0.0,0.0,0.0,0.0 +0,1.579500,0.0,0.0,0.0,0.0,0.0 +0,1.579600,0.0,0.0,0.0,0.0,0.0 +0,1.579700,0.0,0.0,0.0,0.0,0.0 +0,1.579800,0.0,0.0,0.0,0.0,0.0 +0,1.579900,0.0,0.0,0.0,0.0,0.0 +0,1.580000,0.0,0.0,0.0,0.0,0.0 +0,1.580100,0.0,0.0,0.0,0.0,0.0 +1,1643.931451,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.580200,0.0,0.0,0.0,0.0,0.0 +0,1.580300,0.0,0.0,0.0,0.0,0.0 +0,1.580400,0.0,0.0,0.0,0.0,0.0 +0,1.580500,0.0,0.0,0.0,0.0,0.0 +0,1.580600,0.0,0.0,0.0,0.0,0.0 +0,1.580700,0.0,0.0,0.0,0.0,0.0 +0,1.580800,0.0,0.0,0.0,0.0,0.0 +0,1.580900,0.0,0.0,0.0,0.0,0.0 +0,1.581000,0.0,0.0,0.0,0.0,0.0 +0,1.581100,0.0,0.0,0.0,0.0,0.0 +0,1.581200,0.0,0.0,0.0,0.0,0.0 +0,1.581300,0.0,0.0,0.0,0.0,0.0 +0,1.581400,0.0,0.0,0.0,0.0,0.0 +0,1.581500,0.0,0.0,0.0,0.0,0.0 +0,1.581600,0.0,0.0,0.0,0.0,0.0 +0,1.581700,0.0,0.0,0.0,0.0,0.0 +0,1.581800,0.0,0.0,0.0,0.0,0.0 +0,1.581900,0.0,0.0,0.0,0.0,0.0 +0,1.582000,0.0,0.0,0.0,0.0,0.0 +0,1.582100,0.0,0.0,0.0,0.0,0.0 +0,1.582200,0.0,0.0,0.0,0.0,0.0 +0,1.582300,0.0,0.0,0.0,0.0,0.0 +0,1.582400,0.0,0.0,0.0,0.0,0.0 +0,1.582500,0.0,0.0,0.0,0.0,0.0 +0,1.582600,0.0,0.0,0.0,0.0,0.0 +0,1.582700,0.0,0.0,0.0,0.0,0.0 +0,1.582800,0.0,0.0,0.0,0.0,0.0 +0,1.582900,0.0,0.0,0.0,0.0,0.0 +0,1.583000,0.0,0.0,0.0,0.0,0.0 +0,1.583100,0.0,0.0,0.0,0.0,0.0 +0,1.583200,0.0,0.0,0.0,0.0,0.0 +0,1.583300,0.0,0.0,0.0,0.0,0.0 +0,1.583400,0.0,0.0,0.0,0.0,0.0 +0,1.583500,0.0,0.0,0.0,0.0,0.0 +0,1.583600,0.0,0.0,0.0,0.0,0.0 +0,1.583700,0.0,0.0,0.0,0.0,0.0 +0,1.583800,0.0,0.0,0.0,0.0,0.0 +0,1.583900,0.0,0.0,0.0,0.0,0.0 +0,1.584000,0.0,0.0,0.0,0.0,0.0 +0,1.584100,0.0,0.0,0.0,0.0,0.0 +0,1.584200,0.0,0.0,0.0,0.0,0.0 +0,1.584300,0.0,0.0,0.0,0.0,0.0 +0,1.584400,0.0,0.0,0.0,0.0,0.0 +0,1.584500,0.0,0.0,0.0,0.0,0.0 +0,1.584600,0.0,0.0,0.0,0.0,0.0 +0,1.584700,0.0,0.0,0.0,0.0,0.0 +0,1.584800,0.0,0.0,0.0,0.0,0.0 +0,1.584900,0.0,0.0,0.0,0.0,0.0 +0,1.585000,0.0,0.0,0.0,0.0,0.0 +0,1.585100,0.0,0.0,0.0,0.0,0.0 +0,1.585200,0.0,0.0,0.0,0.0,0.0 +0,1.585300,0.0,0.0,0.0,0.0,0.0 +0,1.585400,0.0,0.0,0.0,0.0,0.0 +0,1.585500,0.0,0.0,0.0,0.0,0.0 +0,1.585600,0.0,0.0,0.0,0.0,0.0 +0,1.585700,0.0,0.0,0.0,0.0,0.0 +0,1.585800,0.0,0.0,0.0,0.0,0.0 +0,1.585900,0.0,0.0,0.0,0.0,0.0 +0,1.586000,0.0,0.0,0.0,0.0,0.0 +0,1.586100,0.0,0.0,0.0,0.0,0.0 +0,1.586200,0.0,0.0,0.0,0.0,0.0 +0,1.586300,0.0,0.0,0.0,0.0,0.0 +0,1.586400,0.0,0.0,0.0,0.0,0.0 +0,1.586500,0.0,0.0,0.0,0.0,0.0 +0,1.586600,0.0,0.0,0.0,0.0,0.0 +0,1.586700,0.0,0.0,0.0,0.0,0.0 +0,1.586800,0.0,0.0,0.0,0.0,0.0 +0,1.586900,0.0,0.0,0.0,0.0,0.0 +0,1.587000,0.0,0.0,0.0,0.0,0.0 +0,1.587100,0.0,0.0,0.0,0.0,0.0 +0,1.587200,0.0,0.0,0.0,0.0,0.0 +0,1.587300,0.0,0.0,0.0,0.0,0.0 +0,1.587400,0.0,0.0,0.0,0.0,0.0 +0,1.587500,0.0,0.0,0.0,0.0,0.0 +0,1.587600,0.0,0.0,0.0,0.0,0.0 +0,1.587700,0.0,0.0,0.0,0.0,0.0 +0,1.587800,0.0,0.0,0.0,0.0,0.0 +0,1.587900,0.0,0.0,0.0,0.0,0.0 +0,1.588000,0.0,0.0,0.0,0.0,0.0 +0,1.588100,0.0,0.0,0.0,0.0,0.0 +0,1.588200,0.0,0.0,0.0,0.0,0.0 +0,1.588300,0.0,0.0,0.0,0.0,0.0 +0,1.588400,0.0,0.0,0.0,0.0,0.0 +0,1.588500,0.0,0.0,0.0,0.0,0.0 +0,1.588600,0.0,0.0,0.0,0.0,0.0 +0,1.588700,0.0,0.0,0.0,0.0,0.0 +0,1.588800,0.0,0.0,0.0,0.0,0.0 +0,1.588900,0.0,0.0,0.0,0.0,0.0 +0,1.589000,0.0,0.0,0.0,0.0,0.0 +0,1.589100,0.0,0.0,0.0,0.0,0.0 +0,1.589200,0.0,0.0,0.0,0.0,0.0 +0,1.589300,0.0,0.0,0.0,0.0,0.0 +0,1.589400,0.0,0.0,0.0,0.0,0.0 +0,1.589500,0.0,0.0,0.0,0.0,0.0 +0,1.589600,0.0,0.0,0.0,0.0,0.0 +0,1.589700,0.0,0.0,0.0,0.0,0.0 +0,1.589800,0.0,0.0,0.0,0.0,0.0 +0,1.589900,0.0,0.0,0.0,0.0,0.0 +0,1.590000,0.0,0.0,0.0,0.0,0.0 +0,1.590100,0.0,0.0,0.0,0.0,0.0 +0,1.590200,0.0,0.0,0.0,0.0,0.0 +0,1.590300,0.0,0.0,0.0,0.0,0.0 +0,1.590400,0.0,0.0,0.0,0.0,0.0 +0,1.590500,0.0,0.0,0.0,0.0,0.0 +0,1.590600,0.0,0.0,0.0,0.0,0.0 +0,1.590700,0.0,0.0,0.0,0.0,0.0 +0,1.590800,0.0,0.0,0.0,0.0,0.0 +0,1.590900,0.0,0.0,0.0,0.0,0.0 +0,1.591000,0.0,0.0,0.0,0.0,0.0 +0,1.591100,0.0,0.0,0.0,0.0,0.0 +0,1.591200,0.0,0.0,0.0,0.0,0.0 +0,1.591300,0.0,0.0,0.0,0.0,0.0 +0,1.591400,0.0,0.0,0.0,0.0,0.0 +0,1.591500,0.0,0.0,0.0,0.0,0.0 +0,1.591600,0.0,0.0,0.0,0.0,0.0 +0,1.591700,0.0,0.0,0.0,0.0,0.0 +0,1.591800,0.0,0.0,0.0,0.0,0.0 +0,1.591900,0.0,0.0,0.0,0.0,0.0 +0,1.592000,0.0,0.0,0.0,0.0,0.0 +0,1.592100,0.0,0.0,0.0,0.0,0.0 +0,1.592200,0.0,0.0,0.0,0.0,0.0 +0,1.592300,0.0,0.0,0.0,0.0,0.0 +0,1.592400,0.0,0.0,0.0,0.0,0.0 +0,1.592500,0.0,0.0,0.0,0.0,0.0 +0,1.592600,0.0,0.0,0.0,0.0,0.0 +0,1.592700,0.0,0.0,0.0,0.0,0.0 +0,1.592800,0.0,0.0,0.0,0.0,0.0 +0,1.592900,0.0,0.0,0.0,0.0,0.0 +0,1.593000,0.0,0.0,0.0,0.0,0.0 +0,1.593100,0.0,0.0,0.0,0.0,0.0 +0,1.593200,0.0,0.0,0.0,0.0,0.0 +0,1.593300,0.0,0.0,0.0,0.0,0.0 +0,1.593400,0.0,0.0,0.0,0.0,0.0 +0,1.593500,0.0,0.0,0.0,0.0,0.0 +0,1.593600,0.0,0.0,0.0,0.0,0.0 +0,1.593700,0.0,0.0,0.0,0.0,0.0 +0,1.593800,0.0,0.0,0.0,0.0,0.0 +0,1.593900,0.0,0.0,0.0,0.0,0.0 +0,1.594000,0.0,0.0,0.0,0.0,0.0 +0,1.594100,0.0,0.0,0.0,0.0,0.0 +0,1.594200,0.0,0.0,0.0,0.0,0.0 +0,1.594300,0.0,0.0,0.0,0.0,0.0 +0,1.594400,0.0,0.0,0.0,0.0,0.0 +0,1.594500,0.0,0.0,0.0,0.0,0.0 +0,1.594600,0.0,0.0,0.0,0.0,0.0 +0,1.594700,0.0,0.0,0.0,0.0,0.0 +0,1.594800,0.0,0.0,0.0,0.0,0.0 +0,1.594900,0.0,0.0,0.0,0.0,0.0 +0,1.595000,0.0,0.0,0.0,0.0,0.0 +0,1.595100,0.0,0.0,0.0,0.0,0.0 +0,1.595200,0.0,0.0,0.0,0.0,0.0 +0,1.595300,0.0,0.0,0.0,0.0,0.0 +0,1.595400,0.0,0.0,0.0,0.0,0.0 +0,1.595500,0.0,0.0,0.0,0.0,0.0 +0,1.595600,0.0,0.0,0.0,0.0,0.0 +0,1.595700,0.0,0.0,0.0,0.0,0.0 +0,1.595800,0.0,0.0,0.0,0.0,0.0 +0,1.595900,0.0,0.0,0.0,0.0,0.0 +0,1.596000,0.0,0.0,0.0,0.0,0.0 +0,1.596100,0.0,0.0,0.0,0.0,0.0 +0,1.596200,0.0,0.0,0.0,0.0,0.0 +0,1.596300,0.0,0.0,0.0,0.0,0.0 +0,1.596400,0.0,0.0,0.0,0.0,0.0 +0,1.596500,0.0,0.0,0.0,0.0,0.0 +0,1.596600,0.0,0.0,0.0,0.0,0.0 +0,1.596700,0.0,0.0,0.0,0.0,0.0 +0,1.596800,0.0,0.0,0.0,0.0,0.0 +0,1.596900,0.0,0.0,0.0,0.0,0.0 +0,1.597000,0.0,0.0,0.0,0.0,0.0 +0,1.597100,0.0,0.0,0.0,0.0,0.0 +0,1.597200,0.0,0.0,0.0,0.0,0.0 +0,1.597300,0.0,0.0,0.0,0.0,0.0 +0,1.597400,0.0,0.0,0.0,0.0,0.0 +0,1.597500,0.0,0.0,0.0,0.0,0.0 +0,1.597600,0.0,0.0,0.0,0.0,0.0 +0,1.597700,0.0,0.0,0.0,0.0,0.0 +0,1.597800,0.0,0.0,0.0,0.0,0.0 +0,1.597900,0.0,0.0,0.0,0.0,0.0 +0,1.598000,0.0,0.0,0.0,0.0,0.0 +0,1.598100,0.0,0.0,0.0,0.0,0.0 +0,1.598200,0.0,0.0,0.0,0.0,0.0 +0,1.598300,0.0,0.0,0.0,0.0,0.0 +0,1.598400,0.0,0.0,0.0,0.0,0.0 +0,1.598500,0.0,0.0,0.0,0.0,0.0 +0,1.598600,0.0,0.0,0.0,0.0,0.0 +0,1.598700,0.0,0.0,0.0,0.0,0.0 +0,1.598800,0.0,0.0,0.0,0.0,0.0 +0,1.598900,0.0,0.0,0.0,0.0,0.0 +0,1.599000,0.0,0.0,0.0,0.0,0.0 +0,1.599100,0.0,0.0,0.0,0.0,0.0 +0,1.599200,0.0,0.0,0.0,0.0,0.0 +0,1.599300,0.0,0.0,0.0,0.0,0.0 +0,1.599400,0.0,0.0,0.0,0.0,0.0 +0,1.599500,0.0,0.0,0.0,0.0,0.0 +0,1.599600,0.0,0.0,0.0,0.0,0.0 +0,1.599700,0.0,0.0,0.0,0.0,0.0 +0,1.599800,0.0,0.0,0.0,0.0,0.0 +0,1.599900,0.0,0.0,0.0,0.0,0.0 +0,1.600000,0.0,0.0,0.0,0.0,0.0 +0,1.600100,0.0,0.0,0.0,0.0,0.0 +1,1707.146710,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.600200,0.0,0.0,0.0,0.0,0.0 +0,1.600300,0.0,0.0,0.0,0.0,0.0 +0,1.600400,0.0,0.0,0.0,0.0,0.0 +0,1.600500,0.0,0.0,0.0,0.0,0.0 +0,1.600600,0.0,0.0,0.0,0.0,0.0 +0,1.600700,0.0,0.0,0.0,0.0,0.0 +0,1.600800,0.0,0.0,0.0,0.0,0.0 +0,1.600900,0.0,0.0,0.0,0.0,0.0 +0,1.601000,0.0,0.0,0.0,0.0,0.0 +0,1.601100,0.0,0.0,0.0,0.0,0.0 +0,1.601200,0.0,0.0,0.0,0.0,0.0 +0,1.601300,0.0,0.0,0.0,0.0,0.0 +0,1.601400,0.0,0.0,0.0,0.0,0.0 +0,1.601500,0.0,0.0,0.0,0.0,0.0 +0,1.601600,0.0,0.0,0.0,0.0,0.0 +0,1.601700,0.0,0.0,0.0,0.0,0.0 +0,1.601800,0.0,0.0,0.0,0.0,0.0 +0,1.601900,0.0,0.0,0.0,0.0,0.0 +0,1.602000,0.0,0.0,0.0,0.0,0.0 +0,1.602100,0.0,0.0,0.0,0.0,0.0 +0,1.602200,0.0,0.0,0.0,0.0,0.0 +0,1.602300,0.0,0.0,0.0,0.0,0.0 +0,1.602400,0.0,0.0,0.0,0.0,0.0 +0,1.602500,0.0,0.0,0.0,0.0,0.0 +0,1.602600,0.0,0.0,0.0,0.0,0.0 +0,1.602700,0.0,0.0,0.0,0.0,0.0 +0,1.602800,0.0,0.0,0.0,0.0,0.0 +0,1.602900,0.0,0.0,0.0,0.0,0.0 +0,1.603000,0.0,0.0,0.0,0.0,0.0 +0,1.603100,0.0,0.0,0.0,0.0,0.0 +0,1.603200,0.0,0.0,0.0,0.0,0.0 +0,1.603300,0.0,0.0,0.0,0.0,0.0 +0,1.603400,0.0,0.0,0.0,0.0,0.0 +0,1.603500,0.0,0.0,0.0,0.0,0.0 +0,1.603600,0.0,0.0,0.0,0.0,0.0 +0,1.603700,0.0,0.0,0.0,0.0,0.0 +0,1.603800,0.0,0.0,0.0,0.0,0.0 +0,1.603900,0.0,0.0,0.0,0.0,0.0 +0,1.604000,0.0,0.0,0.0,0.0,0.0 +0,1.604100,0.0,0.0,0.0,0.0,0.0 +0,1.604200,0.0,0.0,0.0,0.0,0.0 +0,1.604300,0.0,0.0,0.0,0.0,0.0 +0,1.604400,0.0,0.0,0.0,0.0,0.0 +0,1.604500,0.0,0.0,0.0,0.0,0.0 +0,1.604600,0.0,0.0,0.0,0.0,0.0 +0,1.604700,0.0,0.0,0.0,0.0,0.0 +0,1.604800,0.0,0.0,0.0,0.0,0.0 +0,1.604900,0.0,0.0,0.0,0.0,0.0 +0,1.605000,0.0,0.0,0.0,0.0,0.0 +0,1.605100,0.0,0.0,0.0,0.0,0.0 +0,1.605200,0.0,0.0,0.0,0.0,0.0 +0,1.605300,0.0,0.0,0.0,0.0,0.0 +0,1.605400,0.0,0.0,0.0,0.0,0.0 +0,1.605500,0.0,0.0,0.0,0.0,0.0 +0,1.605600,0.0,0.0,0.0,0.0,0.0 +0,1.605700,0.0,0.0,0.0,0.0,0.0 +0,1.605800,0.0,0.0,0.0,0.0,0.0 +0,1.605900,0.0,0.0,0.0,0.0,0.0 +0,1.606000,0.0,0.0,0.0,0.0,0.0 +0,1.606100,0.0,0.0,0.0,0.0,0.0 +0,1.606200,0.0,0.0,0.0,0.0,0.0 +0,1.606300,0.0,0.0,0.0,0.0,0.0 +0,1.606400,0.0,0.0,0.0,0.0,0.0 +0,1.606500,0.0,0.0,0.0,0.0,0.0 +0,1.606600,0.0,0.0,0.0,0.0,0.0 +0,1.606700,0.0,0.0,0.0,0.0,0.0 +0,1.606800,0.0,0.0,0.0,0.0,0.0 +0,1.606900,0.0,0.0,0.0,0.0,0.0 +0,1.607000,0.0,0.0,0.0,0.0,0.0 +0,1.607100,0.0,0.0,0.0,0.0,0.0 +0,1.607200,0.0,0.0,0.0,0.0,0.0 +0,1.607300,0.0,0.0,0.0,0.0,0.0 +0,1.607400,0.0,0.0,0.0,0.0,0.0 +0,1.607500,0.0,0.0,0.0,0.0,0.0 +0,1.607600,0.0,0.0,0.0,0.0,0.0 +0,1.607700,0.0,0.0,0.0,0.0,0.0 +0,1.607800,0.0,0.0,0.0,0.0,0.0 +0,1.607900,0.0,0.0,0.0,0.0,0.0 +0,1.608000,0.0,0.0,0.0,0.0,0.0 +0,1.608100,0.0,0.0,0.0,0.0,0.0 +0,1.608200,0.0,0.0,0.0,0.0,0.0 +0,1.608300,0.0,0.0,0.0,0.0,0.0 +0,1.608400,0.0,0.0,0.0,0.0,0.0 +0,1.608500,0.0,0.0,0.0,0.0,0.0 +0,1.608600,0.0,0.0,0.0,0.0,0.0 +0,1.608700,0.0,0.0,0.0,0.0,0.0 +0,1.608800,0.0,0.0,0.0,0.0,0.0 +0,1.608900,0.0,0.0,0.0,0.0,0.0 +0,1.609000,0.0,0.0,0.0,0.0,0.0 +0,1.609100,0.0,0.0,0.0,0.0,0.0 +0,1.609200,0.0,0.0,0.0,0.0,0.0 +0,1.609300,0.0,0.0,0.0,0.0,0.0 +0,1.609400,0.0,0.0,0.0,0.0,0.0 +0,1.609500,0.0,0.0,0.0,0.0,0.0 +0,1.609600,0.0,0.0,0.0,0.0,0.0 +0,1.609700,0.0,0.0,0.0,0.0,0.0 +0,1.609800,0.0,0.0,0.0,0.0,0.0 +0,1.609900,0.0,0.0,0.0,0.0,0.0 +0,1.610000,0.0,0.0,0.0,0.0,0.0 +0,1.610100,0.0,0.0,0.0,0.0,0.0 +0,1.610200,0.0,0.0,0.0,0.0,0.0 +0,1.610300,0.0,0.0,0.0,0.0,0.0 +0,1.610400,0.0,0.0,0.0,0.0,0.0 +0,1.610500,0.0,0.0,0.0,0.0,0.0 +0,1.610600,0.0,0.0,0.0,0.0,0.0 +0,1.610700,0.0,0.0,0.0,0.0,0.0 +0,1.610800,0.0,0.0,0.0,0.0,0.0 +0,1.610900,0.0,0.0,0.0,0.0,0.0 +0,1.611000,0.0,0.0,0.0,0.0,0.0 +0,1.611100,0.0,0.0,0.0,0.0,0.0 +0,1.611200,0.0,0.0,0.0,0.0,0.0 +0,1.611300,0.0,0.0,0.0,0.0,0.0 +0,1.611400,0.0,0.0,0.0,0.0,0.0 +0,1.611500,0.0,0.0,0.0,0.0,0.0 +0,1.611600,0.0,0.0,0.0,0.0,0.0 +0,1.611700,0.0,0.0,0.0,0.0,0.0 +0,1.611800,0.0,0.0,0.0,0.0,0.0 +0,1.611900,0.0,0.0,0.0,0.0,0.0 +0,1.612000,0.0,0.0,0.0,0.0,0.0 +0,1.612100,0.0,0.0,0.0,0.0,0.0 +0,1.612200,0.0,0.0,0.0,0.0,0.0 +0,1.612300,0.0,0.0,0.0,0.0,0.0 +0,1.612400,0.0,0.0,0.0,0.0,0.0 +0,1.612500,0.0,0.0,0.0,0.0,0.0 +0,1.612600,0.0,0.0,0.0,0.0,0.0 +0,1.612700,0.0,0.0,0.0,0.0,0.0 +0,1.612800,0.0,0.0,0.0,0.0,0.0 +0,1.612900,0.0,0.0,0.0,0.0,0.0 +0,1.613000,0.0,0.0,0.0,0.0,0.0 +0,1.613100,0.0,0.0,0.0,0.0,0.0 +0,1.613200,0.0,0.0,0.0,0.0,0.0 +0,1.613300,0.0,0.0,0.0,0.0,0.0 +0,1.613400,0.0,0.0,0.0,0.0,0.0 +0,1.613500,0.0,0.0,0.0,0.0,0.0 +0,1.613600,0.0,0.0,0.0,0.0,0.0 +0,1.613700,0.0,0.0,0.0,0.0,0.0 +0,1.613800,0.0,0.0,0.0,0.0,0.0 +0,1.613900,0.0,0.0,0.0,0.0,0.0 +0,1.614000,0.0,0.0,0.0,0.0,0.0 +0,1.614100,0.0,0.0,0.0,0.0,0.0 +0,1.614200,0.0,0.0,0.0,0.0,0.0 +0,1.614300,0.0,0.0,0.0,0.0,0.0 +0,1.614400,0.0,0.0,0.0,0.0,0.0 +0,1.614500,0.0,0.0,0.0,0.0,0.0 +0,1.614600,0.0,0.0,0.0,0.0,0.0 +0,1.614700,0.0,0.0,0.0,0.0,0.0 +0,1.614800,0.0,0.0,0.0,0.0,0.0 +0,1.614900,0.0,0.0,0.0,0.0,0.0 +0,1.615000,0.0,0.0,0.0,0.0,0.0 +0,1.615100,0.0,0.0,0.0,0.0,0.0 +0,1.615200,0.0,0.0,0.0,0.0,0.0 +0,1.615300,0.0,0.0,0.0,0.0,0.0 +0,1.615400,0.0,0.0,0.0,0.0,0.0 +0,1.615500,0.0,0.0,0.0,0.0,0.0 +0,1.615600,0.0,0.0,0.0,0.0,0.0 +0,1.615700,0.0,0.0,0.0,0.0,0.0 +0,1.615800,0.0,0.0,0.0,0.0,0.0 +0,1.615900,0.0,0.0,0.0,0.0,0.0 +0,1.616000,0.0,0.0,0.0,0.0,0.0 +0,1.616100,0.0,0.0,0.0,0.0,0.0 +0,1.616200,0.0,0.0,0.0,0.0,0.0 +0,1.616300,0.0,0.0,0.0,0.0,0.0 +0,1.616400,0.0,0.0,0.0,0.0,0.0 +0,1.616500,0.0,0.0,0.0,0.0,0.0 +0,1.616600,0.0,0.0,0.0,0.0,0.0 +0,1.616700,0.0,0.0,0.0,0.0,0.0 +0,1.616800,0.0,0.0,0.0,0.0,0.0 +0,1.616900,0.0,0.0,0.0,0.0,0.0 +0,1.617000,0.0,0.0,0.0,0.0,0.0 +0,1.617100,0.0,0.0,0.0,0.0,0.0 +0,1.617200,0.0,0.0,0.0,0.0,0.0 +0,1.617300,0.0,0.0,0.0,0.0,0.0 +0,1.617400,0.0,0.0,0.0,0.0,0.0 +0,1.617500,0.0,0.0,0.0,0.0,0.0 +0,1.617600,0.0,0.0,0.0,0.0,0.0 +0,1.617700,0.0,0.0,0.0,0.0,0.0 +0,1.617800,0.0,0.0,0.0,0.0,0.0 +0,1.617900,0.0,0.0,0.0,0.0,0.0 +0,1.618000,0.0,0.0,0.0,0.0,0.0 +0,1.618100,0.0,0.0,0.0,0.0,0.0 +0,1.618200,0.0,0.0,0.0,0.0,0.0 +0,1.618300,0.0,0.0,0.0,0.0,0.0 +0,1.618400,0.0,0.0,0.0,0.0,0.0 +0,1.618500,0.0,0.0,0.0,0.0,0.0 +0,1.618600,0.0,0.0,0.0,0.0,0.0 +0,1.618700,0.0,0.0,0.0,0.0,0.0 +0,1.618800,0.0,0.0,0.0,0.0,0.0 +0,1.618900,0.0,0.0,0.0,0.0,0.0 +0,1.619000,0.0,0.0,0.0,0.0,0.0 +0,1.619100,0.0,0.0,0.0,0.0,0.0 +0,1.619200,0.0,0.0,0.0,0.0,0.0 +0,1.619300,0.0,0.0,0.0,0.0,0.0 +0,1.619400,0.0,0.0,0.0,0.0,0.0 +0,1.619500,0.0,0.0,0.0,0.0,0.0 +0,1.619600,0.0,0.0,0.0,0.0,0.0 +0,1.619700,0.0,0.0,0.0,0.0,0.0 +0,1.619800,0.0,0.0,0.0,0.0,0.0 +0,1.619900,0.0,0.0,0.0,0.0,0.0 +0,1.620000,0.0,0.0,0.0,0.0,0.0 +0,1.620100,0.0,0.0,0.0,0.0,0.0 +1,1771.962119,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.620200,0.0,0.0,0.0,0.0,0.0 +0,1.620300,0.0,0.0,0.0,0.0,0.0 +0,1.620400,0.0,0.0,0.0,0.0,0.0 +0,1.620500,0.0,0.0,0.0,0.0,0.0 +0,1.620600,0.0,0.0,0.0,0.0,0.0 +0,1.620700,0.0,0.0,0.0,0.0,0.0 +0,1.620800,0.0,0.0,0.0,0.0,0.0 +0,1.620900,0.0,0.0,0.0,0.0,0.0 +0,1.621000,0.0,0.0,0.0,0.0,0.0 +0,1.621100,0.0,0.0,0.0,0.0,0.0 +0,1.621200,0.0,0.0,0.0,0.0,0.0 +0,1.621300,0.0,0.0,0.0,0.0,0.0 +0,1.621400,0.0,0.0,0.0,0.0,0.0 +0,1.621500,0.0,0.0,0.0,0.0,0.0 +0,1.621600,0.0,0.0,0.0,0.0,0.0 +0,1.621700,0.0,0.0,0.0,0.0,0.0 +0,1.621800,0.0,0.0,0.0,0.0,0.0 +0,1.621900,0.0,0.0,0.0,0.0,0.0 +0,1.622000,0.0,0.0,0.0,0.0,0.0 +0,1.622100,0.0,0.0,0.0,0.0,0.0 +0,1.622200,0.0,0.0,0.0,0.0,0.0 +0,1.622300,0.0,0.0,0.0,0.0,0.0 +0,1.622400,0.0,0.0,0.0,0.0,0.0 +0,1.622500,0.0,0.0,0.0,0.0,0.0 +0,1.622600,0.0,0.0,0.0,0.0,0.0 +0,1.622700,0.0,0.0,0.0,0.0,0.0 +0,1.622800,0.0,0.0,0.0,0.0,0.0 +0,1.622900,0.0,0.0,0.0,0.0,0.0 +0,1.623000,0.0,0.0,0.0,0.0,0.0 +0,1.623100,0.0,0.0,0.0,0.0,0.0 +0,1.623200,0.0,0.0,0.0,0.0,0.0 +0,1.623300,0.0,0.0,0.0,0.0,0.0 +0,1.623400,0.0,0.0,0.0,0.0,0.0 +0,1.623500,0.0,0.0,0.0,0.0,0.0 +0,1.623600,0.0,0.0,0.0,0.0,0.0 +0,1.623700,0.0,0.0,0.0,0.0,0.0 +0,1.623800,0.0,0.0,0.0,0.0,0.0 +0,1.623900,0.0,0.0,0.0,0.0,0.0 +0,1.624000,0.0,0.0,0.0,0.0,0.0 +0,1.624100,0.0,0.0,0.0,0.0,0.0 +0,1.624200,0.0,0.0,0.0,0.0,0.0 +0,1.624300,0.0,0.0,0.0,0.0,0.0 +0,1.624400,0.0,0.0,0.0,0.0,0.0 +0,1.624500,0.0,0.0,0.0,0.0,0.0 +0,1.624600,0.0,0.0,0.0,0.0,0.0 +0,1.624700,0.0,0.0,0.0,0.0,0.0 +0,1.624800,0.0,0.0,0.0,0.0,0.0 +0,1.624900,0.0,0.0,0.0,0.0,0.0 +0,1.625000,0.0,0.0,0.0,0.0,0.0 +0,1.625100,0.0,0.0,0.0,0.0,0.0 +0,1.625200,0.0,0.0,0.0,0.0,0.0 +0,1.625300,0.0,0.0,0.0,0.0,0.0 +0,1.625400,0.0,0.0,0.0,0.0,0.0 +0,1.625500,0.0,0.0,0.0,0.0,0.0 +0,1.625600,0.0,0.0,0.0,0.0,0.0 +0,1.625700,0.0,0.0,0.0,0.0,0.0 +0,1.625800,0.0,0.0,0.0,0.0,0.0 +0,1.625900,0.0,0.0,0.0,0.0,0.0 +0,1.626000,0.0,0.0,0.0,0.0,0.0 +0,1.626100,0.0,0.0,0.0,0.0,0.0 +0,1.626200,0.0,0.0,0.0,0.0,0.0 +0,1.626300,0.0,0.0,0.0,0.0,0.0 +0,1.626400,0.0,0.0,0.0,0.0,0.0 +0,1.626500,0.0,0.0,0.0,0.0,0.0 +0,1.626600,0.0,0.0,0.0,0.0,0.0 +0,1.626700,0.0,0.0,0.0,0.0,0.0 +0,1.626800,0.0,0.0,0.0,0.0,0.0 +0,1.626900,0.0,0.0,0.0,0.0,0.0 +0,1.627000,0.0,0.0,0.0,0.0,0.0 +0,1.627100,0.0,0.0,0.0,0.0,0.0 +0,1.627200,0.0,0.0,0.0,0.0,0.0 +0,1.627300,0.0,0.0,0.0,0.0,0.0 +0,1.627400,0.0,0.0,0.0,0.0,0.0 +0,1.627500,0.0,0.0,0.0,0.0,0.0 +0,1.627600,0.0,0.0,0.0,0.0,0.0 +0,1.627700,0.0,0.0,0.0,0.0,0.0 +0,1.627800,0.0,0.0,0.0,0.0,0.0 +0,1.627900,0.0,0.0,0.0,0.0,0.0 +0,1.628000,0.0,0.0,0.0,0.0,0.0 +0,1.628100,0.0,0.0,0.0,0.0,0.0 +0,1.628200,0.0,0.0,0.0,0.0,0.0 +0,1.628300,0.0,0.0,0.0,0.0,0.0 +0,1.628400,0.0,0.0,0.0,0.0,0.0 +0,1.628500,0.0,0.0,0.0,0.0,0.0 +0,1.628600,0.0,0.0,0.0,0.0,0.0 +0,1.628700,0.0,0.0,0.0,0.0,0.0 +0,1.628800,0.0,0.0,0.0,0.0,0.0 +0,1.628900,0.0,0.0,0.0,0.0,0.0 +0,1.629000,0.0,0.0,0.0,0.0,0.0 +0,1.629100,0.0,0.0,0.0,0.0,0.0 +0,1.629200,0.0,0.0,0.0,0.0,0.0 +0,1.629300,0.0,0.0,0.0,0.0,0.0 +0,1.629400,0.0,0.0,0.0,0.0,0.0 +0,1.629500,0.0,0.0,0.0,0.0,0.0 +0,1.629600,0.0,0.0,0.0,0.0,0.0 +0,1.629700,0.0,0.0,0.0,0.0,0.0 +0,1.629800,0.0,0.0,0.0,0.0,0.0 +0,1.629900,0.0,0.0,0.0,0.0,0.0 +0,1.630000,0.0,0.0,0.0,0.0,0.0 +0,1.630100,0.0,0.0,0.0,0.0,0.0 +0,1.630200,0.0,0.0,0.0,0.0,0.0 +0,1.630300,0.0,0.0,0.0,0.0,0.0 +0,1.630400,0.0,0.0,0.0,0.0,0.0 +0,1.630500,0.0,0.0,0.0,0.0,0.0 +0,1.630600,0.0,0.0,0.0,0.0,0.0 +0,1.630700,0.0,0.0,0.0,0.0,0.0 +0,1.630800,0.0,0.0,0.0,0.0,0.0 +0,1.630900,0.0,0.0,0.0,0.0,0.0 +0,1.631000,0.0,0.0,0.0,0.0,0.0 +0,1.631100,0.0,0.0,0.0,0.0,0.0 +0,1.631200,0.0,0.0,0.0,0.0,0.0 +0,1.631300,0.0,0.0,0.0,0.0,0.0 +0,1.631400,0.0,0.0,0.0,0.0,0.0 +0,1.631500,0.0,0.0,0.0,0.0,0.0 +0,1.631600,0.0,0.0,0.0,0.0,0.0 +0,1.631700,0.0,0.0,0.0,0.0,0.0 +0,1.631800,0.0,0.0,0.0,0.0,0.0 +0,1.631900,0.0,0.0,0.0,0.0,0.0 +0,1.632000,0.0,0.0,0.0,0.0,0.0 +0,1.632100,0.0,0.0,0.0,0.0,0.0 +0,1.632200,0.0,0.0,0.0,0.0,0.0 +0,1.632300,0.0,0.0,0.0,0.0,0.0 +0,1.632400,0.0,0.0,0.0,0.0,0.0 +0,1.632500,0.0,0.0,0.0,0.0,0.0 +0,1.632600,0.0,0.0,0.0,0.0,0.0 +0,1.632700,0.0,0.0,0.0,0.0,0.0 +0,1.632800,0.0,0.0,0.0,0.0,0.0 +0,1.632900,0.0,0.0,0.0,0.0,0.0 +0,1.633000,0.0,0.0,0.0,0.0,0.0 +0,1.633100,0.0,0.0,0.0,0.0,0.0 +0,1.633200,0.0,0.0,0.0,0.0,0.0 +0,1.633300,0.0,0.0,0.0,0.0,0.0 +0,1.633400,0.0,0.0,0.0,0.0,0.0 +0,1.633500,0.0,0.0,0.0,0.0,0.0 +0,1.633600,0.0,0.0,0.0,0.0,0.0 +0,1.633700,0.0,0.0,0.0,0.0,0.0 +0,1.633800,0.0,0.0,0.0,0.0,0.0 +0,1.633900,0.0,0.0,0.0,0.0,0.0 +0,1.634000,0.0,0.0,0.0,0.0,0.0 +0,1.634100,0.0,0.0,0.0,0.0,0.0 +0,1.634200,0.0,0.0,0.0,0.0,0.0 +0,1.634300,0.0,0.0,0.0,0.0,0.0 +0,1.634400,0.0,0.0,0.0,0.0,0.0 +0,1.634500,0.0,0.0,0.0,0.0,0.0 +0,1.634600,0.0,0.0,0.0,0.0,0.0 +0,1.634700,0.0,0.0,0.0,0.0,0.0 +0,1.634800,0.0,0.0,0.0,0.0,0.0 +0,1.634900,0.0,0.0,0.0,0.0,0.0 +0,1.635000,0.0,0.0,0.0,0.0,0.0 +0,1.635100,0.0,0.0,0.0,0.0,0.0 +0,1.635200,0.0,0.0,0.0,0.0,0.0 +0,1.635300,0.0,0.0,0.0,0.0,0.0 +0,1.635400,0.0,0.0,0.0,0.0,0.0 +0,1.635500,0.0,0.0,0.0,0.0,0.0 +0,1.635600,0.0,0.0,0.0,0.0,0.0 +0,1.635700,0.0,0.0,0.0,0.0,0.0 +0,1.635800,0.0,0.0,0.0,0.0,0.0 +0,1.635900,0.0,0.0,0.0,0.0,0.0 +0,1.636000,0.0,0.0,0.0,0.0,0.0 +0,1.636100,0.0,0.0,0.0,0.0,0.0 +0,1.636200,0.0,0.0,0.0,0.0,0.0 +0,1.636300,0.0,0.0,0.0,0.0,0.0 +0,1.636400,0.0,0.0,0.0,0.0,0.0 +0,1.636500,0.0,0.0,0.0,0.0,0.0 +0,1.636600,0.0,0.0,0.0,0.0,0.0 +0,1.636700,0.0,0.0,0.0,0.0,0.0 +0,1.636800,0.0,0.0,0.0,0.0,0.0 +0,1.636900,0.0,0.0,0.0,0.0,0.0 +0,1.637000,0.0,0.0,0.0,0.0,0.0 +0,1.637100,0.0,0.0,0.0,0.0,0.0 +0,1.637200,0.0,0.0,0.0,0.0,0.0 +0,1.637300,0.0,0.0,0.0,0.0,0.0 +0,1.637400,0.0,0.0,0.0,0.0,0.0 +0,1.637500,0.0,0.0,0.0,0.0,0.0 +0,1.637600,0.0,0.0,0.0,0.0,0.0 +0,1.637700,0.0,0.0,0.0,0.0,0.0 +0,1.637800,0.0,0.0,0.0,0.0,0.0 +0,1.637900,0.0,0.0,0.0,0.0,0.0 +0,1.638000,0.0,0.0,0.0,0.0,0.0 +0,1.638100,0.0,0.0,0.0,0.0,0.0 +0,1.638200,0.0,0.0,0.0,0.0,0.0 +0,1.638300,0.0,0.0,0.0,0.0,0.0 +0,1.638400,0.0,0.0,0.0,0.0,0.0 +0,1.638500,0.0,0.0,0.0,0.0,0.0 +0,1.638600,0.0,0.0,0.0,0.0,0.0 +0,1.638700,0.0,0.0,0.0,0.0,0.0 +0,1.638800,0.0,0.0,0.0,0.0,0.0 +0,1.638900,0.0,0.0,0.0,0.0,0.0 +0,1.639000,0.0,0.0,0.0,0.0,0.0 +0,1.639100,0.0,0.0,0.0,0.0,0.0 +0,1.639200,0.0,0.0,0.0,0.0,0.0 +0,1.639300,0.0,0.0,0.0,0.0,0.0 +0,1.639400,0.0,0.0,0.0,0.0,0.0 +0,1.639500,0.0,0.0,0.0,0.0,0.0 +0,1.639600,0.0,0.0,0.0,0.0,0.0 +0,1.639700,0.0,0.0,0.0,0.0,0.0 +0,1.639800,0.0,0.0,0.0,0.0,0.0 +0,1.639900,0.0,0.0,0.0,0.0,0.0 +0,1.640000,0.0,0.0,0.0,0.0,0.0 +0,1.640100,0.0,0.0,0.0,0.0,0.0 +1,1838.397678,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.640200,0.0,0.0,0.0,0.0,0.0 +0,1.640300,0.0,0.0,0.0,0.0,0.0 +0,1.640400,0.0,0.0,0.0,0.0,0.0 +0,1.640500,0.0,0.0,0.0,0.0,0.0 +0,1.640600,0.0,0.0,0.0,0.0,0.0 +0,1.640700,0.0,0.0,0.0,0.0,0.0 +0,1.640800,0.0,0.0,0.0,0.0,0.0 +0,1.640900,0.0,0.0,0.0,0.0,0.0 +0,1.641000,0.0,0.0,0.0,0.0,0.0 +0,1.641100,0.0,0.0,0.0,0.0,0.0 +0,1.641200,0.0,0.0,0.0,0.0,0.0 +0,1.641300,0.0,0.0,0.0,0.0,0.0 +0,1.641400,0.0,0.0,0.0,0.0,0.0 +0,1.641500,0.0,0.0,0.0,0.0,0.0 +0,1.641600,0.0,0.0,0.0,0.0,0.0 +0,1.641700,0.0,0.0,0.0,0.0,0.0 +0,1.641800,0.0,0.0,0.0,0.0,0.0 +0,1.641900,0.0,0.0,0.0,0.0,0.0 +0,1.642000,0.0,0.0,0.0,0.0,0.0 +0,1.642100,0.0,0.0,0.0,0.0,0.0 +0,1.642200,0.0,0.0,0.0,0.0,0.0 +0,1.642300,0.0,0.0,0.0,0.0,0.0 +0,1.642400,0.0,0.0,0.0,0.0,0.0 +0,1.642500,0.0,0.0,0.0,0.0,0.0 +0,1.642600,0.0,0.0,0.0,0.0,0.0 +0,1.642700,0.0,0.0,0.0,0.0,0.0 +0,1.642800,0.0,0.0,0.0,0.0,0.0 +0,1.642900,0.0,0.0,0.0,0.0,0.0 +0,1.643000,0.0,0.0,0.0,0.0,0.0 +0,1.643100,0.0,0.0,0.0,0.0,0.0 +0,1.643200,0.0,0.0,0.0,0.0,0.0 +0,1.643300,0.0,0.0,0.0,0.0,0.0 +0,1.643400,0.0,0.0,0.0,0.0,0.0 +0,1.643500,0.0,0.0,0.0,0.0,0.0 +0,1.643600,0.0,0.0,0.0,0.0,0.0 +0,1.643700,0.0,0.0,0.0,0.0,0.0 +0,1.643800,0.0,0.0,0.0,0.0,0.0 +0,1.643900,0.0,0.0,0.0,0.0,0.0 +0,1.644000,0.0,0.0,0.0,0.0,0.0 +0,1.644100,0.0,0.0,0.0,0.0,0.0 +0,1.644200,0.0,0.0,0.0,0.0,0.0 +0,1.644300,0.0,0.0,0.0,0.0,0.0 +0,1.644400,0.0,0.0,0.0,0.0,0.0 +0,1.644500,0.0,0.0,0.0,0.0,0.0 +0,1.644600,0.0,0.0,0.0,0.0,0.0 +0,1.644700,0.0,0.0,0.0,0.0,0.0 +0,1.644800,0.0,0.0,0.0,0.0,0.0 +0,1.644900,0.0,0.0,0.0,0.0,0.0 +0,1.645000,0.0,0.0,0.0,0.0,0.0 +0,1.645100,0.0,0.0,0.0,0.0,0.0 +0,1.645200,0.0,0.0,0.0,0.0,0.0 +0,1.645300,0.0,0.0,0.0,0.0,0.0 +0,1.645400,0.0,0.0,0.0,0.0,0.0 +0,1.645500,0.0,0.0,0.0,0.0,0.0 +0,1.645600,0.0,0.0,0.0,0.0,0.0 +0,1.645700,0.0,0.0,0.0,0.0,0.0 +0,1.645800,0.0,0.0,0.0,0.0,0.0 +0,1.645900,0.0,0.0,0.0,0.0,0.0 +0,1.646000,0.0,0.0,0.0,0.0,0.0 +0,1.646100,0.0,0.0,0.0,0.0,0.0 +0,1.646200,0.0,0.0,0.0,0.0,0.0 +0,1.646300,0.0,0.0,0.0,0.0,0.0 +0,1.646400,0.0,0.0,0.0,0.0,0.0 +0,1.646500,0.0,0.0,0.0,0.0,0.0 +0,1.646600,0.0,0.0,0.0,0.0,0.0 +0,1.646700,0.0,0.0,0.0,0.0,0.0 +0,1.646800,0.0,0.0,0.0,0.0,0.0 +0,1.646900,0.0,0.0,0.0,0.0,0.0 +0,1.647000,0.0,0.0,0.0,0.0,0.0 +0,1.647100,0.0,0.0,0.0,0.0,0.0 +0,1.647200,0.0,0.0,0.0,0.0,0.0 +0,1.647300,0.0,0.0,0.0,0.0,0.0 +0,1.647400,0.0,0.0,0.0,0.0,0.0 +0,1.647500,0.0,0.0,0.0,0.0,0.0 +0,1.647600,0.0,0.0,0.0,0.0,0.0 +0,1.647700,0.0,0.0,0.0,0.0,0.0 +0,1.647800,0.0,0.0,0.0,0.0,0.0 +0,1.647900,0.0,0.0,0.0,0.0,0.0 +0,1.648000,0.0,0.0,0.0,0.0,0.0 +0,1.648100,0.0,0.0,0.0,0.0,0.0 +0,1.648200,0.0,0.0,0.0,0.0,0.0 +0,1.648300,0.0,0.0,0.0,0.0,0.0 +0,1.648400,0.0,0.0,0.0,0.0,0.0 +0,1.648500,0.0,0.0,0.0,0.0,0.0 +0,1.648600,0.0,0.0,0.0,0.0,0.0 +0,1.648700,0.0,0.0,0.0,0.0,0.0 +0,1.648800,0.0,0.0,0.0,0.0,0.0 +0,1.648900,0.0,0.0,0.0,0.0,0.0 +0,1.649000,0.0,0.0,0.0,0.0,0.0 +0,1.649100,0.0,0.0,0.0,0.0,0.0 +0,1.649200,0.0,0.0,0.0,0.0,0.0 +0,1.649300,0.0,0.0,0.0,0.0,0.0 +0,1.649400,0.0,0.0,0.0,0.0,0.0 +0,1.649500,0.0,0.0,0.0,0.0,0.0 +0,1.649600,0.0,0.0,0.0,0.0,0.0 +0,1.649700,0.0,0.0,0.0,0.0,0.0 +0,1.649800,0.0,0.0,0.0,0.0,0.0 +0,1.649900,0.0,0.0,0.0,0.0,0.0 +0,1.650000,0.0,0.0,0.0,0.0,0.0 +0,1.650100,0.0,0.0,0.0,0.0,0.0 +0,1.650200,0.0,0.0,0.0,0.0,0.0 +0,1.650300,0.0,0.0,0.0,0.0,0.0 +0,1.650400,0.0,0.0,0.0,0.0,0.0 +0,1.650500,0.0,0.0,0.0,0.0,0.0 +0,1.650600,0.0,0.0,0.0,0.0,0.0 +0,1.650700,0.0,0.0,0.0,0.0,0.0 +0,1.650800,0.0,0.0,0.0,0.0,0.0 +0,1.650900,0.0,0.0,0.0,0.0,0.0 +0,1.651000,0.0,0.0,0.0,0.0,0.0 +0,1.651100,0.0,0.0,0.0,0.0,0.0 +0,1.651200,0.0,0.0,0.0,0.0,0.0 +0,1.651300,0.0,0.0,0.0,0.0,0.0 +0,1.651400,0.0,0.0,0.0,0.0,0.0 +0,1.651500,0.0,0.0,0.0,0.0,0.0 +0,1.651600,0.0,0.0,0.0,0.0,0.0 +0,1.651700,0.0,0.0,0.0,0.0,0.0 +0,1.651800,0.0,0.0,0.0,0.0,0.0 +0,1.651900,0.0,0.0,0.0,0.0,0.0 +0,1.652000,0.0,0.0,0.0,0.0,0.0 +0,1.652100,0.0,0.0,0.0,0.0,0.0 +0,1.652200,0.0,0.0,0.0,0.0,0.0 +0,1.652300,0.0,0.0,0.0,0.0,0.0 +0,1.652400,0.0,0.0,0.0,0.0,0.0 +0,1.652500,0.0,0.0,0.0,0.0,0.0 +0,1.652600,0.0,0.0,0.0,0.0,0.0 +0,1.652700,0.0,0.0,0.0,0.0,0.0 +0,1.652800,0.0,0.0,0.0,0.0,0.0 +0,1.652900,0.0,0.0,0.0,0.0,0.0 +0,1.653000,0.0,0.0,0.0,0.0,0.0 +0,1.653100,0.0,0.0,0.0,0.0,0.0 +0,1.653200,0.0,0.0,0.0,0.0,0.0 +0,1.653300,0.0,0.0,0.0,0.0,0.0 +0,1.653400,0.0,0.0,0.0,0.0,0.0 +0,1.653500,0.0,0.0,0.0,0.0,0.0 +0,1.653600,0.0,0.0,0.0,0.0,0.0 +0,1.653700,0.0,0.0,0.0,0.0,0.0 +0,1.653800,0.0,0.0,0.0,0.0,0.0 +0,1.653900,0.0,0.0,0.0,0.0,0.0 +0,1.654000,0.0,0.0,0.0,0.0,0.0 +0,1.654100,0.0,0.0,0.0,0.0,0.0 +0,1.654200,0.0,0.0,0.0,0.0,0.0 +0,1.654300,0.0,0.0,0.0,0.0,0.0 +0,1.654400,0.0,0.0,0.0,0.0,0.0 +0,1.654500,0.0,0.0,0.0,0.0,0.0 +0,1.654600,0.0,0.0,0.0,0.0,0.0 +0,1.654700,0.0,0.0,0.0,0.0,0.0 +0,1.654800,0.0,0.0,0.0,0.0,0.0 +0,1.654900,0.0,0.0,0.0,0.0,0.0 +0,1.655000,0.0,0.0,0.0,0.0,0.0 +0,1.655100,0.0,0.0,0.0,0.0,0.0 +0,1.655200,0.0,0.0,0.0,0.0,0.0 +0,1.655300,0.0,0.0,0.0,0.0,0.0 +0,1.655400,0.0,0.0,0.0,0.0,0.0 +0,1.655500,0.0,0.0,0.0,0.0,0.0 +0,1.655600,0.0,0.0,0.0,0.0,0.0 +0,1.655700,0.0,0.0,0.0,0.0,0.0 +0,1.655800,0.0,0.0,0.0,0.0,0.0 +0,1.655900,0.0,0.0,0.0,0.0,0.0 +0,1.656000,0.0,0.0,0.0,0.0,0.0 +0,1.656100,0.0,0.0,0.0,0.0,0.0 +0,1.656200,0.0,0.0,0.0,0.0,0.0 +0,1.656300,0.0,0.0,0.0,0.0,0.0 +0,1.656400,0.0,0.0,0.0,0.0,0.0 +0,1.656500,0.0,0.0,0.0,0.0,0.0 +0,1.656600,0.0,0.0,0.0,0.0,0.0 +0,1.656700,0.0,0.0,0.0,0.0,0.0 +0,1.656800,0.0,0.0,0.0,0.0,0.0 +0,1.656900,0.0,0.0,0.0,0.0,0.0 +0,1.657000,0.0,0.0,0.0,0.0,0.0 +0,1.657100,0.0,0.0,0.0,0.0,0.0 +0,1.657200,0.0,0.0,0.0,0.0,0.0 +0,1.657300,0.0,0.0,0.0,0.0,0.0 +0,1.657400,0.0,0.0,0.0,0.0,0.0 +0,1.657500,0.0,0.0,0.0,0.0,0.0 +0,1.657600,0.0,0.0,0.0,0.0,0.0 +0,1.657700,0.0,0.0,0.0,0.0,0.0 +0,1.657800,0.0,0.0,0.0,0.0,0.0 +0,1.657900,0.0,0.0,0.0,0.0,0.0 +0,1.658000,0.0,0.0,0.0,0.0,0.0 +0,1.658100,0.0,0.0,0.0,0.0,0.0 +0,1.658200,0.0,0.0,0.0,0.0,0.0 +0,1.658300,0.0,0.0,0.0,0.0,0.0 +0,1.658400,0.0,0.0,0.0,0.0,0.0 +0,1.658500,0.0,0.0,0.0,0.0,0.0 +0,1.658600,0.0,0.0,0.0,0.0,0.0 +0,1.658700,0.0,0.0,0.0,0.0,0.0 +0,1.658800,0.0,0.0,0.0,0.0,0.0 +0,1.658900,0.0,0.0,0.0,0.0,0.0 +0,1.659000,0.0,0.0,0.0,0.0,0.0 +0,1.659100,0.0,0.0,0.0,0.0,0.0 +0,1.659200,0.0,0.0,0.0,0.0,0.0 +0,1.659300,0.0,0.0,0.0,0.0,0.0 +0,1.659400,0.0,0.0,0.0,0.0,0.0 +0,1.659500,0.0,0.0,0.0,0.0,0.0 +0,1.659600,0.0,0.0,0.0,0.0,0.0 +0,1.659700,0.0,0.0,0.0,0.0,0.0 +0,1.659800,0.0,0.0,0.0,0.0,0.0 +0,1.659900,0.0,0.0,0.0,0.0,0.0 +0,1.660000,0.0,0.0,0.0,0.0,0.0 +0,1.660100,0.0,0.0,0.0,0.0,0.0 +1,1906.473387,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.660200,0.0,0.0,0.0,0.0,0.0 +0,1.660300,0.0,0.0,0.0,0.0,0.0 +0,1.660400,0.0,0.0,0.0,0.0,0.0 +0,1.660500,0.0,0.0,0.0,0.0,0.0 +0,1.660600,0.0,0.0,0.0,0.0,0.0 +0,1.660700,0.0,0.0,0.0,0.0,0.0 +0,1.660800,0.0,0.0,0.0,0.0,0.0 +0,1.660900,0.0,0.0,0.0,0.0,0.0 +0,1.661000,0.0,0.0,0.0,0.0,0.0 +0,1.661100,0.0,0.0,0.0,0.0,0.0 +0,1.661200,0.0,0.0,0.0,0.0,0.0 +0,1.661300,0.0,0.0,0.0,0.0,0.0 +0,1.661400,0.0,0.0,0.0,0.0,0.0 +0,1.661500,0.0,0.0,0.0,0.0,0.0 +0,1.661600,0.0,0.0,0.0,0.0,0.0 +0,1.661700,0.0,0.0,0.0,0.0,0.0 +0,1.661800,0.0,0.0,0.0,0.0,0.0 +0,1.661900,0.0,0.0,0.0,0.0,0.0 +0,1.662000,0.0,0.0,0.0,0.0,0.0 +0,1.662100,0.0,0.0,0.0,0.0,0.0 +0,1.662200,0.0,0.0,0.0,0.0,0.0 +0,1.662300,0.0,0.0,0.0,0.0,0.0 +0,1.662400,0.0,0.0,0.0,0.0,0.0 +0,1.662500,0.0,0.0,0.0,0.0,0.0 +0,1.662600,0.0,0.0,0.0,0.0,0.0 +0,1.662700,0.0,0.0,0.0,0.0,0.0 +0,1.662800,0.0,0.0,0.0,0.0,0.0 +0,1.662900,0.0,0.0,0.0,0.0,0.0 +0,1.663000,0.0,0.0,0.0,0.0,0.0 +0,1.663100,0.0,0.0,0.0,0.0,0.0 +0,1.663200,0.0,0.0,0.0,0.0,0.0 +0,1.663300,0.0,0.0,0.0,0.0,0.0 +0,1.663400,0.0,0.0,0.0,0.0,0.0 +0,1.663500,0.0,0.0,0.0,0.0,0.0 +0,1.663600,0.0,0.0,0.0,0.0,0.0 +0,1.663700,0.0,0.0,0.0,0.0,0.0 +0,1.663800,0.0,0.0,0.0,0.0,0.0 +0,1.663900,0.0,0.0,0.0,0.0,0.0 +0,1.664000,0.0,0.0,0.0,0.0,0.0 +0,1.664100,0.0,0.0,0.0,0.0,0.0 +0,1.664200,0.0,0.0,0.0,0.0,0.0 +0,1.664300,0.0,0.0,0.0,0.0,0.0 +0,1.664400,0.0,0.0,0.0,0.0,0.0 +0,1.664500,0.0,0.0,0.0,0.0,0.0 +0,1.664600,0.0,0.0,0.0,0.0,0.0 +0,1.664700,0.0,0.0,0.0,0.0,0.0 +0,1.664800,0.0,0.0,0.0,0.0,0.0 +0,1.664900,0.0,0.0,0.0,0.0,0.0 +0,1.665000,0.0,0.0,0.0,0.0,0.0 +0,1.665100,0.0,0.0,0.0,0.0,0.0 +0,1.665200,0.0,0.0,0.0,0.0,0.0 +0,1.665300,0.0,0.0,0.0,0.0,0.0 +0,1.665400,0.0,0.0,0.0,0.0,0.0 +0,1.665500,0.0,0.0,0.0,0.0,0.0 +0,1.665600,0.0,0.0,0.0,0.0,0.0 +0,1.665700,0.0,0.0,0.0,0.0,0.0 +0,1.665800,0.0,0.0,0.0,0.0,0.0 +0,1.665900,0.0,0.0,0.0,0.0,0.0 +0,1.666000,0.0,0.0,0.0,0.0,0.0 +0,1.666100,0.0,0.0,0.0,0.0,0.0 +0,1.666200,0.0,0.0,0.0,0.0,0.0 +0,1.666300,0.0,0.0,0.0,0.0,0.0 +0,1.666400,0.0,0.0,0.0,0.0,0.0 +0,1.666500,0.0,0.0,0.0,0.0,0.0 +0,1.666600,0.0,0.0,0.0,0.0,0.0 +0,1.666700,0.0,0.0,0.0,0.0,0.0 +0,1.666800,0.0,0.0,0.0,0.0,0.0 +0,1.666900,0.0,0.0,0.0,0.0,0.0 +0,1.667000,0.0,0.0,0.0,0.0,0.0 +0,1.667100,0.0,0.0,0.0,0.0,0.0 +0,1.667200,0.0,0.0,0.0,0.0,0.0 +0,1.667300,0.0,0.0,0.0,0.0,0.0 +0,1.667400,0.0,0.0,0.0,0.0,0.0 +0,1.667500,0.0,0.0,0.0,0.0,0.0 +0,1.667600,0.0,0.0,0.0,0.0,0.0 +0,1.667700,0.0,0.0,0.0,0.0,0.0 +0,1.667800,0.0,0.0,0.0,0.0,0.0 +0,1.667900,0.0,0.0,0.0,0.0,0.0 +0,1.668000,0.0,0.0,0.0,0.0,0.0 +0,1.668100,0.0,0.0,0.0,0.0,0.0 +0,1.668200,0.0,0.0,0.0,0.0,0.0 +0,1.668300,0.0,0.0,0.0,0.0,0.0 +0,1.668400,0.0,0.0,0.0,0.0,0.0 +0,1.668500,0.0,0.0,0.0,0.0,0.0 +0,1.668600,0.0,0.0,0.0,0.0,0.0 +0,1.668700,0.0,0.0,0.0,0.0,0.0 +0,1.668800,0.0,0.0,0.0,0.0,0.0 +0,1.668900,0.0,0.0,0.0,0.0,0.0 +0,1.669000,0.0,0.0,0.0,0.0,0.0 +0,1.669100,0.0,0.0,0.0,0.0,0.0 +0,1.669200,0.0,0.0,0.0,0.0,0.0 +0,1.669300,0.0,0.0,0.0,0.0,0.0 +0,1.669400,0.0,0.0,0.0,0.0,0.0 +0,1.669500,0.0,0.0,0.0,0.0,0.0 +0,1.669600,0.0,0.0,0.0,0.0,0.0 +0,1.669700,0.0,0.0,0.0,0.0,0.0 +0,1.669800,0.0,0.0,0.0,0.0,0.0 +0,1.669900,0.0,0.0,0.0,0.0,0.0 +0,1.670000,0.0,0.0,0.0,0.0,0.0 +0,1.670100,0.0,0.0,0.0,0.0,0.0 +0,1.670200,0.0,0.0,0.0,0.0,0.0 +0,1.670300,0.0,0.0,0.0,0.0,0.0 +0,1.670400,0.0,0.0,0.0,0.0,0.0 +0,1.670500,0.0,0.0,0.0,0.0,0.0 +0,1.670600,0.0,0.0,0.0,0.0,0.0 +0,1.670700,0.0,0.0,0.0,0.0,0.0 +0,1.670800,0.0,0.0,0.0,0.0,0.0 +0,1.670900,0.0,0.0,0.0,0.0,0.0 +0,1.671000,0.0,0.0,0.0,0.0,0.0 +0,1.671100,0.0,0.0,0.0,0.0,0.0 +0,1.671200,0.0,0.0,0.0,0.0,0.0 +0,1.671300,0.0,0.0,0.0,0.0,0.0 +0,1.671400,0.0,0.0,0.0,0.0,0.0 +0,1.671500,0.0,0.0,0.0,0.0,0.0 +0,1.671600,0.0,0.0,0.0,0.0,0.0 +0,1.671700,0.0,0.0,0.0,0.0,0.0 +0,1.671800,0.0,0.0,0.0,0.0,0.0 +0,1.671900,0.0,0.0,0.0,0.0,0.0 +0,1.672000,0.0,0.0,0.0,0.0,0.0 +0,1.672100,0.0,0.0,0.0,0.0,0.0 +0,1.672200,0.0,0.0,0.0,0.0,0.0 +0,1.672300,0.0,0.0,0.0,0.0,0.0 +0,1.672400,0.0,0.0,0.0,0.0,0.0 +0,1.672500,0.0,0.0,0.0,0.0,0.0 +0,1.672600,0.0,0.0,0.0,0.0,0.0 +0,1.672700,0.0,0.0,0.0,0.0,0.0 +0,1.672800,0.0,0.0,0.0,0.0,0.0 +0,1.672900,0.0,0.0,0.0,0.0,0.0 +0,1.673000,0.0,0.0,0.0,0.0,0.0 +0,1.673100,0.0,0.0,0.0,0.0,0.0 +0,1.673200,0.0,0.0,0.0,0.0,0.0 +0,1.673300,0.0,0.0,0.0,0.0,0.0 +0,1.673400,0.0,0.0,0.0,0.0,0.0 +0,1.673500,0.0,0.0,0.0,0.0,0.0 +0,1.673600,0.0,0.0,0.0,0.0,0.0 +0,1.673700,0.0,0.0,0.0,0.0,0.0 +0,1.673800,0.0,0.0,0.0,0.0,0.0 +0,1.673900,0.0,0.0,0.0,0.0,0.0 +0,1.674000,0.0,0.0,0.0,0.0,0.0 +0,1.674100,0.0,0.0,0.0,0.0,0.0 +0,1.674200,0.0,0.0,0.0,0.0,0.0 +0,1.674300,0.0,0.0,0.0,0.0,0.0 +0,1.674400,0.0,0.0,0.0,0.0,0.0 +0,1.674500,0.0,0.0,0.0,0.0,0.0 +0,1.674600,0.0,0.0,0.0,0.0,0.0 +0,1.674700,0.0,0.0,0.0,0.0,0.0 +0,1.674800,0.0,0.0,0.0,0.0,0.0 +0,1.674900,0.0,0.0,0.0,0.0,0.0 +0,1.675000,0.0,0.0,0.0,0.0,0.0 +0,1.675100,0.0,0.0,0.0,0.0,0.0 +0,1.675200,0.0,0.0,0.0,0.0,0.0 +0,1.675300,0.0,0.0,0.0,0.0,0.0 +0,1.675400,0.0,0.0,0.0,0.0,0.0 +0,1.675500,0.0,0.0,0.0,0.0,0.0 +0,1.675600,0.0,0.0,0.0,0.0,0.0 +0,1.675700,0.0,0.0,0.0,0.0,0.0 +0,1.675800,0.0,0.0,0.0,0.0,0.0 +0,1.675900,0.0,0.0,0.0,0.0,0.0 +0,1.676000,0.0,0.0,0.0,0.0,0.0 +0,1.676100,0.0,0.0,0.0,0.0,0.0 +0,1.676200,0.0,0.0,0.0,0.0,0.0 +0,1.676300,0.0,0.0,0.0,0.0,0.0 +0,1.676400,0.0,0.0,0.0,0.0,0.0 +0,1.676500,0.0,0.0,0.0,0.0,0.0 +0,1.676600,0.0,0.0,0.0,0.0,0.0 +0,1.676700,0.0,0.0,0.0,0.0,0.0 +0,1.676800,0.0,0.0,0.0,0.0,0.0 +0,1.676900,0.0,0.0,0.0,0.0,0.0 +0,1.677000,0.0,0.0,0.0,0.0,0.0 +0,1.677100,0.0,0.0,0.0,0.0,0.0 +0,1.677200,0.0,0.0,0.0,0.0,0.0 +0,1.677300,0.0,0.0,0.0,0.0,0.0 +0,1.677400,0.0,0.0,0.0,0.0,0.0 +0,1.677500,0.0,0.0,0.0,0.0,0.0 +0,1.677600,0.0,0.0,0.0,0.0,0.0 +0,1.677700,0.0,0.0,0.0,0.0,0.0 +0,1.677800,0.0,0.0,0.0,0.0,0.0 +0,1.677900,0.0,0.0,0.0,0.0,0.0 +0,1.678000,0.0,0.0,0.0,0.0,0.0 +0,1.678100,0.0,0.0,0.0,0.0,0.0 +0,1.678200,0.0,0.0,0.0,0.0,0.0 +0,1.678300,0.0,0.0,0.0,0.0,0.0 +0,1.678400,0.0,0.0,0.0,0.0,0.0 +0,1.678500,0.0,0.0,0.0,0.0,0.0 +0,1.678600,0.0,0.0,0.0,0.0,0.0 +0,1.678700,0.0,0.0,0.0,0.0,0.0 +0,1.678800,0.0,0.0,0.0,0.0,0.0 +0,1.678900,0.0,0.0,0.0,0.0,0.0 +0,1.679000,0.0,0.0,0.0,0.0,0.0 +0,1.679100,0.0,0.0,0.0,0.0,0.0 +0,1.679200,0.0,0.0,0.0,0.0,0.0 +0,1.679300,0.0,0.0,0.0,0.0,0.0 +0,1.679400,0.0,0.0,0.0,0.0,0.0 +0,1.679500,0.0,0.0,0.0,0.0,0.0 +0,1.679600,0.0,0.0,0.0,0.0,0.0 +0,1.679700,0.0,0.0,0.0,0.0,0.0 +0,1.679800,0.0,0.0,0.0,0.0,0.0 +0,1.679900,0.0,0.0,0.0,0.0,0.0 +0,1.680000,0.0,0.0,0.0,0.0,0.0 +0,1.680100,0.0,0.0,0.0,0.0,0.0 +1,1976.209246,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.680200,0.0,0.0,0.0,0.0,0.0 +0,1.680300,0.0,0.0,0.0,0.0,0.0 +0,1.680400,0.0,0.0,0.0,0.0,0.0 +0,1.680500,0.0,0.0,0.0,0.0,0.0 +0,1.680600,0.0,0.0,0.0,0.0,0.0 +0,1.680700,0.0,0.0,0.0,0.0,0.0 +0,1.680800,0.0,0.0,0.0,0.0,0.0 +0,1.680900,0.0,0.0,0.0,0.0,0.0 +0,1.681000,0.0,0.0,0.0,0.0,0.0 +0,1.681100,0.0,0.0,0.0,0.0,0.0 +0,1.681200,0.0,0.0,0.0,0.0,0.0 +0,1.681300,0.0,0.0,0.0,0.0,0.0 +0,1.681400,0.0,0.0,0.0,0.0,0.0 +0,1.681500,0.0,0.0,0.0,0.0,0.0 +0,1.681600,0.0,0.0,0.0,0.0,0.0 +0,1.681700,0.0,0.0,0.0,0.0,0.0 +0,1.681800,0.0,0.0,0.0,0.0,0.0 +0,1.681900,0.0,0.0,0.0,0.0,0.0 +0,1.682000,0.0,0.0,0.0,0.0,0.0 +0,1.682100,0.0,0.0,0.0,0.0,0.0 +0,1.682200,0.0,0.0,0.0,0.0,0.0 +0,1.682300,0.0,0.0,0.0,0.0,0.0 +0,1.682400,0.0,0.0,0.0,0.0,0.0 +0,1.682500,0.0,0.0,0.0,0.0,0.0 +0,1.682600,0.0,0.0,0.0,0.0,0.0 +0,1.682700,0.0,0.0,0.0,0.0,0.0 +0,1.682800,0.0,0.0,0.0,0.0,0.0 +0,1.682900,0.0,0.0,0.0,0.0,0.0 +0,1.683000,0.0,0.0,0.0,0.0,0.0 +0,1.683100,0.0,0.0,0.0,0.0,0.0 +0,1.683200,0.0,0.0,0.0,0.0,0.0 +0,1.683300,0.0,0.0,0.0,0.0,0.0 +0,1.683400,0.0,0.0,0.0,0.0,0.0 +0,1.683500,0.0,0.0,0.0,0.0,0.0 +0,1.683600,0.0,0.0,0.0,0.0,0.0 +0,1.683700,0.0,0.0,0.0,0.0,0.0 +0,1.683800,0.0,0.0,0.0,0.0,0.0 +0,1.683900,0.0,0.0,0.0,0.0,0.0 +0,1.684000,0.0,0.0,0.0,0.0,0.0 +0,1.684100,0.0,0.0,0.0,0.0,0.0 +0,1.684200,0.0,0.0,0.0,0.0,0.0 +0,1.684300,0.0,0.0,0.0,0.0,0.0 +0,1.684400,0.0,0.0,0.0,0.0,0.0 +0,1.684500,0.0,0.0,0.0,0.0,0.0 +0,1.684600,0.0,0.0,0.0,0.0,0.0 +0,1.684700,0.0,0.0,0.0,0.0,0.0 +0,1.684800,0.0,0.0,0.0,0.0,0.0 +0,1.684900,0.0,0.0,0.0,0.0,0.0 +0,1.685000,0.0,0.0,0.0,0.0,0.0 +0,1.685100,0.0,0.0,0.0,0.0,0.0 +0,1.685200,0.0,0.0,0.0,0.0,0.0 +0,1.685300,0.0,0.0,0.0,0.0,0.0 +0,1.685400,0.0,0.0,0.0,0.0,0.0 +0,1.685500,0.0,0.0,0.0,0.0,0.0 +0,1.685600,0.0,0.0,0.0,0.0,0.0 +0,1.685700,0.0,0.0,0.0,0.0,0.0 +0,1.685800,0.0,0.0,0.0,0.0,0.0 +0,1.685900,0.0,0.0,0.0,0.0,0.0 +0,1.686000,0.0,0.0,0.0,0.0,0.0 +0,1.686100,0.0,0.0,0.0,0.0,0.0 +0,1.686200,0.0,0.0,0.0,0.0,0.0 +0,1.686300,0.0,0.0,0.0,0.0,0.0 +0,1.686400,0.0,0.0,0.0,0.0,0.0 +0,1.686500,0.0,0.0,0.0,0.0,0.0 +0,1.686600,0.0,0.0,0.0,0.0,0.0 +0,1.686700,0.0,0.0,0.0,0.0,0.0 +0,1.686800,0.0,0.0,0.0,0.0,0.0 +0,1.686900,0.0,0.0,0.0,0.0,0.0 +0,1.687000,0.0,0.0,0.0,0.0,0.0 +0,1.687100,0.0,0.0,0.0,0.0,0.0 +0,1.687200,0.0,0.0,0.0,0.0,0.0 +0,1.687300,0.0,0.0,0.0,0.0,0.0 +0,1.687400,0.0,0.0,0.0,0.0,0.0 +0,1.687500,0.0,0.0,0.0,0.0,0.0 +0,1.687600,0.0,0.0,0.0,0.0,0.0 +0,1.687700,0.0,0.0,0.0,0.0,0.0 +0,1.687800,0.0,0.0,0.0,0.0,0.0 +0,1.687900,0.0,0.0,0.0,0.0,0.0 +0,1.688000,0.0,0.0,0.0,0.0,0.0 +0,1.688100,0.0,0.0,0.0,0.0,0.0 +0,1.688200,0.0,0.0,0.0,0.0,0.0 +0,1.688300,0.0,0.0,0.0,0.0,0.0 +0,1.688400,0.0,0.0,0.0,0.0,0.0 +0,1.688500,0.0,0.0,0.0,0.0,0.0 +0,1.688600,0.0,0.0,0.0,0.0,0.0 +0,1.688700,0.0,0.0,0.0,0.0,0.0 +0,1.688800,0.0,0.0,0.0,0.0,0.0 +0,1.688900,0.0,0.0,0.0,0.0,0.0 +0,1.689000,0.0,0.0,0.0,0.0,0.0 +0,1.689100,0.0,0.0,0.0,0.0,0.0 +0,1.689200,0.0,0.0,0.0,0.0,0.0 +0,1.689300,0.0,0.0,0.0,0.0,0.0 +0,1.689400,0.0,0.0,0.0,0.0,0.0 +0,1.689500,0.0,0.0,0.0,0.0,0.0 +0,1.689600,0.0,0.0,0.0,0.0,0.0 +0,1.689700,0.0,0.0,0.0,0.0,0.0 +0,1.689800,0.0,0.0,0.0,0.0,0.0 +0,1.689900,0.0,0.0,0.0,0.0,0.0 +0,1.690000,0.0,0.0,0.0,0.0,0.0 +0,1.690100,0.0,0.0,0.0,0.0,0.0 +0,1.690200,0.0,0.0,0.0,0.0,0.0 +0,1.690300,0.0,0.0,0.0,0.0,0.0 +0,1.690400,0.0,0.0,0.0,0.0,0.0 +0,1.690500,0.0,0.0,0.0,0.0,0.0 +0,1.690600,0.0,0.0,0.0,0.0,0.0 +0,1.690700,0.0,0.0,0.0,0.0,0.0 +0,1.690800,0.0,0.0,0.0,0.0,0.0 +0,1.690900,0.0,0.0,0.0,0.0,0.0 +0,1.691000,0.0,0.0,0.0,0.0,0.0 +0,1.691100,0.0,0.0,0.0,0.0,0.0 +0,1.691200,0.0,0.0,0.0,0.0,0.0 +0,1.691300,0.0,0.0,0.0,0.0,0.0 +0,1.691400,0.0,0.0,0.0,0.0,0.0 +0,1.691500,0.0,0.0,0.0,0.0,0.0 +0,1.691600,0.0,0.0,0.0,0.0,0.0 +0,1.691700,0.0,0.0,0.0,0.0,0.0 +0,1.691800,0.0,0.0,0.0,0.0,0.0 +0,1.691900,0.0,0.0,0.0,0.0,0.0 +0,1.692000,0.0,0.0,0.0,0.0,0.0 +0,1.692100,0.0,0.0,0.0,0.0,0.0 +0,1.692200,0.0,0.0,0.0,0.0,0.0 +0,1.692300,0.0,0.0,0.0,0.0,0.0 +0,1.692400,0.0,0.0,0.0,0.0,0.0 +0,1.692500,0.0,0.0,0.0,0.0,0.0 +0,1.692600,0.0,0.0,0.0,0.0,0.0 +0,1.692700,0.0,0.0,0.0,0.0,0.0 +0,1.692800,0.0,0.0,0.0,0.0,0.0 +0,1.692900,0.0,0.0,0.0,0.0,0.0 +0,1.693000,0.0,0.0,0.0,0.0,0.0 +0,1.693100,0.0,0.0,0.0,0.0,0.0 +0,1.693200,0.0,0.0,0.0,0.0,0.0 +0,1.693300,0.0,0.0,0.0,0.0,0.0 +0,1.693400,0.0,0.0,0.0,0.0,0.0 +0,1.693500,0.0,0.0,0.0,0.0,0.0 +0,1.693600,0.0,0.0,0.0,0.0,0.0 +0,1.693700,0.0,0.0,0.0,0.0,0.0 +0,1.693800,0.0,0.0,0.0,0.0,0.0 +0,1.693900,0.0,0.0,0.0,0.0,0.0 +0,1.694000,0.0,0.0,0.0,0.0,0.0 +0,1.694100,0.0,0.0,0.0,0.0,0.0 +0,1.694200,0.0,0.0,0.0,0.0,0.0 +0,1.694300,0.0,0.0,0.0,0.0,0.0 +0,1.694400,0.0,0.0,0.0,0.0,0.0 +0,1.694500,0.0,0.0,0.0,0.0,0.0 +0,1.694600,0.0,0.0,0.0,0.0,0.0 +0,1.694700,0.0,0.0,0.0,0.0,0.0 +0,1.694800,0.0,0.0,0.0,0.0,0.0 +0,1.694900,0.0,0.0,0.0,0.0,0.0 +0,1.695000,0.0,0.0,0.0,0.0,0.0 +0,1.695100,0.0,0.0,0.0,0.0,0.0 +0,1.695200,0.0,0.0,0.0,0.0,0.0 +0,1.695300,0.0,0.0,0.0,0.0,0.0 +0,1.695400,0.0,0.0,0.0,0.0,0.0 +0,1.695500,0.0,0.0,0.0,0.0,0.0 +0,1.695600,0.0,0.0,0.0,0.0,0.0 +0,1.695700,0.0,0.0,0.0,0.0,0.0 +0,1.695800,0.0,0.0,0.0,0.0,0.0 +0,1.695900,0.0,0.0,0.0,0.0,0.0 +0,1.696000,0.0,0.0,0.0,0.0,0.0 +0,1.696100,0.0,0.0,0.0,0.0,0.0 +0,1.696200,0.0,0.0,0.0,0.0,0.0 +0,1.696300,0.0,0.0,0.0,0.0,0.0 +0,1.696400,0.0,0.0,0.0,0.0,0.0 +0,1.696500,0.0,0.0,0.0,0.0,0.0 +0,1.696600,0.0,0.0,0.0,0.0,0.0 +0,1.696700,0.0,0.0,0.0,0.0,0.0 +0,1.696800,0.0,0.0,0.0,0.0,0.0 +0,1.696900,0.0,0.0,0.0,0.0,0.0 +0,1.697000,0.0,0.0,0.0,0.0,0.0 +0,1.697100,0.0,0.0,0.0,0.0,0.0 +0,1.697200,0.0,0.0,0.0,0.0,0.0 +0,1.697300,0.0,0.0,0.0,0.0,0.0 +0,1.697400,0.0,0.0,0.0,0.0,0.0 +0,1.697500,0.0,0.0,0.0,0.0,0.0 +0,1.697600,0.0,0.0,0.0,0.0,0.0 +0,1.697700,0.0,0.0,0.0,0.0,0.0 +0,1.697800,0.0,0.0,0.0,0.0,0.0 +0,1.697900,0.0,0.0,0.0,0.0,0.0 +0,1.698000,0.0,0.0,0.0,0.0,0.0 +0,1.698100,0.0,0.0,0.0,0.0,0.0 +0,1.698200,0.0,0.0,0.0,0.0,0.0 +0,1.698300,0.0,0.0,0.0,0.0,0.0 +0,1.698400,0.0,0.0,0.0,0.0,0.0 +0,1.698500,0.0,0.0,0.0,0.0,0.0 +0,1.698600,0.0,0.0,0.0,0.0,0.0 +0,1.698700,0.0,0.0,0.0,0.0,0.0 +0,1.698800,0.0,0.0,0.0,0.0,0.0 +0,1.698900,0.0,0.0,0.0,0.0,0.0 +0,1.699000,0.0,0.0,0.0,0.0,0.0 +0,1.699100,0.0,0.0,0.0,0.0,0.0 +0,1.699200,0.0,0.0,0.0,0.0,0.0 +0,1.699300,0.0,0.0,0.0,0.0,0.0 +0,1.699400,0.0,0.0,0.0,0.0,0.0 +0,1.699500,0.0,0.0,0.0,0.0,0.0 +0,1.699600,0.0,0.0,0.0,0.0,0.0 +0,1.699700,0.0,0.0,0.0,0.0,0.0 +0,1.699800,0.0,0.0,0.0,0.0,0.0 +0,1.699900,0.0,0.0,0.0,0.0,0.0 +0,1.700000,0.0,0.0,0.0,0.0,0.0 +0,1.700100,0.0,0.0,0.0,0.0,0.0 +1,2047.625254,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.700200,0.0,0.0,0.0,0.0,0.0 +0,1.700300,0.0,0.0,0.0,0.0,0.0 +0,1.700400,0.0,0.0,0.0,0.0,0.0 +0,1.700500,0.0,0.0,0.0,0.0,0.0 +0,1.700600,0.0,0.0,0.0,0.0,0.0 +0,1.700700,0.0,0.0,0.0,0.0,0.0 +0,1.700800,0.0,0.0,0.0,0.0,0.0 +0,1.700900,0.0,0.0,0.0,0.0,0.0 +0,1.701000,0.0,0.0,0.0,0.0,0.0 +0,1.701100,0.0,0.0,0.0,0.0,0.0 +0,1.701200,0.0,0.0,0.0,0.0,0.0 +0,1.701300,0.0,0.0,0.0,0.0,0.0 +0,1.701400,0.0,0.0,0.0,0.0,0.0 +0,1.701500,0.0,0.0,0.0,0.0,0.0 +0,1.701600,0.0,0.0,0.0,0.0,0.0 +0,1.701700,0.0,0.0,0.0,0.0,0.0 +0,1.701800,0.0,0.0,0.0,0.0,0.0 +0,1.701900,0.0,0.0,0.0,0.0,0.0 +0,1.702000,0.0,0.0,0.0,0.0,0.0 +0,1.702100,0.0,0.0,0.0,0.0,0.0 +0,1.702200,0.0,0.0,0.0,0.0,0.0 +0,1.702300,0.0,0.0,0.0,0.0,0.0 +0,1.702400,0.0,0.0,0.0,0.0,0.0 +0,1.702500,0.0,0.0,0.0,0.0,0.0 +0,1.702600,0.0,0.0,0.0,0.0,0.0 +0,1.702700,0.0,0.0,0.0,0.0,0.0 +0,1.702800,0.0,0.0,0.0,0.0,0.0 +0,1.702900,0.0,0.0,0.0,0.0,0.0 +0,1.703000,0.0,0.0,0.0,0.0,0.0 +0,1.703100,0.0,0.0,0.0,0.0,0.0 +0,1.703200,0.0,0.0,0.0,0.0,0.0 +0,1.703300,0.0,0.0,0.0,0.0,0.0 +0,1.703400,0.0,0.0,0.0,0.0,0.0 +0,1.703500,0.0,0.0,0.0,0.0,0.0 +0,1.703600,0.0,0.0,0.0,0.0,0.0 +0,1.703700,0.0,0.0,0.0,0.0,0.0 +0,1.703800,0.0,0.0,0.0,0.0,0.0 +0,1.703900,0.0,0.0,0.0,0.0,0.0 +0,1.704000,0.0,0.0,0.0,0.0,0.0 +0,1.704100,0.0,0.0,0.0,0.0,0.0 +0,1.704200,0.0,0.0,0.0,0.0,0.0 +0,1.704300,0.0,0.0,0.0,0.0,0.0 +0,1.704400,0.0,0.0,0.0,0.0,0.0 +0,1.704500,0.0,0.0,0.0,0.0,0.0 +0,1.704600,0.0,0.0,0.0,0.0,0.0 +0,1.704700,0.0,0.0,0.0,0.0,0.0 +0,1.704800,0.0,0.0,0.0,0.0,0.0 +0,1.704900,0.0,0.0,0.0,0.0,0.0 +0,1.705000,0.0,0.0,0.0,0.0,0.0 +0,1.705100,0.0,0.0,0.0,0.0,0.0 +0,1.705200,0.0,0.0,0.0,0.0,0.0 +0,1.705300,0.0,0.0,0.0,0.0,0.0 +0,1.705400,0.0,0.0,0.0,0.0,0.0 +0,1.705500,0.0,0.0,0.0,0.0,0.0 +0,1.705600,0.0,0.0,0.0,0.0,0.0 +0,1.705700,0.0,0.0,0.0,0.0,0.0 +0,1.705800,0.0,0.0,0.0,0.0,0.0 +0,1.705900,0.0,0.0,0.0,0.0,0.0 +0,1.706000,0.0,0.0,0.0,0.0,0.0 +0,1.706100,0.0,0.0,0.0,0.0,0.0 +0,1.706200,0.0,0.0,0.0,0.0,0.0 +0,1.706300,0.0,0.0,0.0,0.0,0.0 +0,1.706400,0.0,0.0,0.0,0.0,0.0 +0,1.706500,0.0,0.0,0.0,0.0,0.0 +0,1.706600,0.0,0.0,0.0,0.0,0.0 +0,1.706700,0.0,0.0,0.0,0.0,0.0 +0,1.706800,0.0,0.0,0.0,0.0,0.0 +0,1.706900,0.0,0.0,0.0,0.0,0.0 +0,1.707000,0.0,0.0,0.0,0.0,0.0 +0,1.707100,0.0,0.0,0.0,0.0,0.0 +0,1.707200,0.0,0.0,0.0,0.0,0.0 +0,1.707300,0.0,0.0,0.0,0.0,0.0 +0,1.707400,0.0,0.0,0.0,0.0,0.0 +0,1.707500,0.0,0.0,0.0,0.0,0.0 +0,1.707600,0.0,0.0,0.0,0.0,0.0 +0,1.707700,0.0,0.0,0.0,0.0,0.0 +0,1.707800,0.0,0.0,0.0,0.0,0.0 +0,1.707900,0.0,0.0,0.0,0.0,0.0 +0,1.708000,0.0,0.0,0.0,0.0,0.0 +0,1.708100,0.0,0.0,0.0,0.0,0.0 +0,1.708200,0.0,0.0,0.0,0.0,0.0 +0,1.708300,0.0,0.0,0.0,0.0,0.0 +0,1.708400,0.0,0.0,0.0,0.0,0.0 +0,1.708500,0.0,0.0,0.0,0.0,0.0 +0,1.708600,0.0,0.0,0.0,0.0,0.0 +0,1.708700,0.0,0.0,0.0,0.0,0.0 +0,1.708800,0.0,0.0,0.0,0.0,0.0 +0,1.708900,0.0,0.0,0.0,0.0,0.0 +0,1.709000,0.0,0.0,0.0,0.0,0.0 +0,1.709100,0.0,0.0,0.0,0.0,0.0 +0,1.709200,0.0,0.0,0.0,0.0,0.0 +0,1.709300,0.0,0.0,0.0,0.0,0.0 +0,1.709400,0.0,0.0,0.0,0.0,0.0 +0,1.709500,0.0,0.0,0.0,0.0,0.0 +0,1.709600,0.0,0.0,0.0,0.0,0.0 +0,1.709700,0.0,0.0,0.0,0.0,0.0 +0,1.709800,0.0,0.0,0.0,0.0,0.0 +0,1.709900,0.0,0.0,0.0,0.0,0.0 +0,1.710000,0.0,0.0,0.0,0.0,0.0 +0,1.710100,0.0,0.0,0.0,0.0,0.0 +0,1.710200,0.0,0.0,0.0,0.0,0.0 +0,1.710300,0.0,0.0,0.0,0.0,0.0 +0,1.710400,0.0,0.0,0.0,0.0,0.0 +0,1.710500,0.0,0.0,0.0,0.0,0.0 +0,1.710600,0.0,0.0,0.0,0.0,0.0 +0,1.710700,0.0,0.0,0.0,0.0,0.0 +0,1.710800,0.0,0.0,0.0,0.0,0.0 +0,1.710900,0.0,0.0,0.0,0.0,0.0 +0,1.711000,0.0,0.0,0.0,0.0,0.0 +0,1.711100,0.0,0.0,0.0,0.0,0.0 +0,1.711200,0.0,0.0,0.0,0.0,0.0 +0,1.711300,0.0,0.0,0.0,0.0,0.0 +0,1.711400,0.0,0.0,0.0,0.0,0.0 +0,1.711500,0.0,0.0,0.0,0.0,0.0 +0,1.711600,0.0,0.0,0.0,0.0,0.0 +0,1.711700,0.0,0.0,0.0,0.0,0.0 +0,1.711800,0.0,0.0,0.0,0.0,0.0 +0,1.711900,0.0,0.0,0.0,0.0,0.0 +0,1.712000,0.0,0.0,0.0,0.0,0.0 +0,1.712100,0.0,0.0,0.0,0.0,0.0 +0,1.712200,0.0,0.0,0.0,0.0,0.0 +0,1.712300,0.0,0.0,0.0,0.0,0.0 +0,1.712400,0.0,0.0,0.0,0.0,0.0 +0,1.712500,0.0,0.0,0.0,0.0,0.0 +0,1.712600,0.0,0.0,0.0,0.0,0.0 +0,1.712700,0.0,0.0,0.0,0.0,0.0 +0,1.712800,0.0,0.0,0.0,0.0,0.0 +0,1.712900,0.0,0.0,0.0,0.0,0.0 +0,1.713000,0.0,0.0,0.0,0.0,0.0 +0,1.713100,0.0,0.0,0.0,0.0,0.0 +0,1.713200,0.0,0.0,0.0,0.0,0.0 +0,1.713300,0.0,0.0,0.0,0.0,0.0 +0,1.713400,0.0,0.0,0.0,0.0,0.0 +0,1.713500,0.0,0.0,0.0,0.0,0.0 +0,1.713600,0.0,0.0,0.0,0.0,0.0 +0,1.713700,0.0,0.0,0.0,0.0,0.0 +0,1.713800,0.0,0.0,0.0,0.0,0.0 +0,1.713900,0.0,0.0,0.0,0.0,0.0 +0,1.714000,0.0,0.0,0.0,0.0,0.0 +0,1.714100,0.0,0.0,0.0,0.0,0.0 +0,1.714200,0.0,0.0,0.0,0.0,0.0 +0,1.714300,0.0,0.0,0.0,0.0,0.0 +0,1.714400,0.0,0.0,0.0,0.0,0.0 +0,1.714500,0.0,0.0,0.0,0.0,0.0 +0,1.714600,0.0,0.0,0.0,0.0,0.0 +0,1.714700,0.0,0.0,0.0,0.0,0.0 +0,1.714800,0.0,0.0,0.0,0.0,0.0 +0,1.714900,0.0,0.0,0.0,0.0,0.0 +0,1.715000,0.0,0.0,0.0,0.0,0.0 +0,1.715100,0.0,0.0,0.0,0.0,0.0 +0,1.715200,0.0,0.0,0.0,0.0,0.0 +0,1.715300,0.0,0.0,0.0,0.0,0.0 +0,1.715400,0.0,0.0,0.0,0.0,0.0 +0,1.715500,0.0,0.0,0.0,0.0,0.0 +0,1.715600,0.0,0.0,0.0,0.0,0.0 +0,1.715700,0.0,0.0,0.0,0.0,0.0 +0,1.715800,0.0,0.0,0.0,0.0,0.0 +0,1.715900,0.0,0.0,0.0,0.0,0.0 +0,1.716000,0.0,0.0,0.0,0.0,0.0 +0,1.716100,0.0,0.0,0.0,0.0,0.0 +0,1.716200,0.0,0.0,0.0,0.0,0.0 +0,1.716300,0.0,0.0,0.0,0.0,0.0 +0,1.716400,0.0,0.0,0.0,0.0,0.0 +0,1.716500,0.0,0.0,0.0,0.0,0.0 +0,1.716600,0.0,0.0,0.0,0.0,0.0 +0,1.716700,0.0,0.0,0.0,0.0,0.0 +0,1.716800,0.0,0.0,0.0,0.0,0.0 +0,1.716900,0.0,0.0,0.0,0.0,0.0 +0,1.717000,0.0,0.0,0.0,0.0,0.0 +0,1.717100,0.0,0.0,0.0,0.0,0.0 +0,1.717200,0.0,0.0,0.0,0.0,0.0 +0,1.717300,0.0,0.0,0.0,0.0,0.0 +0,1.717400,0.0,0.0,0.0,0.0,0.0 +0,1.717500,0.0,0.0,0.0,0.0,0.0 +0,1.717600,0.0,0.0,0.0,0.0,0.0 +0,1.717700,0.0,0.0,0.0,0.0,0.0 +0,1.717800,0.0,0.0,0.0,0.0,0.0 +0,1.717900,0.0,0.0,0.0,0.0,0.0 +0,1.718000,0.0,0.0,0.0,0.0,0.0 +0,1.718100,0.0,0.0,0.0,0.0,0.0 +0,1.718200,0.0,0.0,0.0,0.0,0.0 +0,1.718300,0.0,0.0,0.0,0.0,0.0 +0,1.718400,0.0,0.0,0.0,0.0,0.0 +0,1.718500,0.0,0.0,0.0,0.0,0.0 +0,1.718600,0.0,0.0,0.0,0.0,0.0 +0,1.718700,0.0,0.0,0.0,0.0,0.0 +0,1.718800,0.0,0.0,0.0,0.0,0.0 +0,1.718900,0.0,0.0,0.0,0.0,0.0 +0,1.719000,0.0,0.0,0.0,0.0,0.0 +0,1.719100,0.0,0.0,0.0,0.0,0.0 +0,1.719200,0.0,0.0,0.0,0.0,0.0 +0,1.719300,0.0,0.0,0.0,0.0,0.0 +0,1.719400,0.0,0.0,0.0,0.0,0.0 +0,1.719500,0.0,0.0,0.0,0.0,0.0 +0,1.719600,0.0,0.0,0.0,0.0,0.0 +0,1.719700,0.0,0.0,0.0,0.0,0.0 +0,1.719800,0.0,0.0,0.0,0.0,0.0 +0,1.719900,0.0,0.0,0.0,0.0,0.0 +0,1.720000,0.0,0.0,0.0,0.0,0.0 +0,1.720100,0.0,0.0,0.0,0.0,0.0 +1,2120.741413,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.720200,0.0,0.0,0.0,0.0,0.0 +0,1.720300,0.0,0.0,0.0,0.0,0.0 +0,1.720400,0.0,0.0,0.0,0.0,0.0 +0,1.720500,0.0,0.0,0.0,0.0,0.0 +0,1.720600,0.0,0.0,0.0,0.0,0.0 +0,1.720700,0.0,0.0,0.0,0.0,0.0 +0,1.720800,0.0,0.0,0.0,0.0,0.0 +0,1.720900,0.0,0.0,0.0,0.0,0.0 +0,1.721000,0.0,0.0,0.0,0.0,0.0 +0,1.721100,0.0,0.0,0.0,0.0,0.0 +0,1.721200,0.0,0.0,0.0,0.0,0.0 +0,1.721300,0.0,0.0,0.0,0.0,0.0 +0,1.721400,0.0,0.0,0.0,0.0,0.0 +0,1.721500,0.0,0.0,0.0,0.0,0.0 +0,1.721600,0.0,0.0,0.0,0.0,0.0 +0,1.721700,0.0,0.0,0.0,0.0,0.0 +0,1.721800,0.0,0.0,0.0,0.0,0.0 +0,1.721900,0.0,0.0,0.0,0.0,0.0 +0,1.722000,0.0,0.0,0.0,0.0,0.0 +0,1.722100,0.0,0.0,0.0,0.0,0.0 +0,1.722200,0.0,0.0,0.0,0.0,0.0 +0,1.722300,0.0,0.0,0.0,0.0,0.0 +0,1.722400,0.0,0.0,0.0,0.0,0.0 +0,1.722500,0.0,0.0,0.0,0.0,0.0 +0,1.722600,0.0,0.0,0.0,0.0,0.0 +0,1.722700,0.0,0.0,0.0,0.0,0.0 +0,1.722800,0.0,0.0,0.0,0.0,0.0 +0,1.722900,0.0,0.0,0.0,0.0,0.0 +0,1.723000,0.0,0.0,0.0,0.0,0.0 +0,1.723100,0.0,0.0,0.0,0.0,0.0 +0,1.723200,0.0,0.0,0.0,0.0,0.0 +0,1.723300,0.0,0.0,0.0,0.0,0.0 +0,1.723400,0.0,0.0,0.0,0.0,0.0 +0,1.723500,0.0,0.0,0.0,0.0,0.0 +0,1.723600,0.0,0.0,0.0,0.0,0.0 +0,1.723700,0.0,0.0,0.0,0.0,0.0 +0,1.723800,0.0,0.0,0.0,0.0,0.0 +0,1.723900,0.0,0.0,0.0,0.0,0.0 +0,1.724000,0.0,0.0,0.0,0.0,0.0 +0,1.724100,0.0,0.0,0.0,0.0,0.0 +0,1.724200,0.0,0.0,0.0,0.0,0.0 +0,1.724300,0.0,0.0,0.0,0.0,0.0 +0,1.724400,0.0,0.0,0.0,0.0,0.0 +0,1.724500,0.0,0.0,0.0,0.0,0.0 +0,1.724600,0.0,0.0,0.0,0.0,0.0 +0,1.724700,0.0,0.0,0.0,0.0,0.0 +0,1.724800,0.0,0.0,0.0,0.0,0.0 +0,1.724900,0.0,0.0,0.0,0.0,0.0 +0,1.725000,0.0,0.0,0.0,0.0,0.0 +0,1.725100,0.0,0.0,0.0,0.0,0.0 +0,1.725200,0.0,0.0,0.0,0.0,0.0 +0,1.725300,0.0,0.0,0.0,0.0,0.0 +0,1.725400,0.0,0.0,0.0,0.0,0.0 +0,1.725500,0.0,0.0,0.0,0.0,0.0 +0,1.725600,0.0,0.0,0.0,0.0,0.0 +0,1.725700,0.0,0.0,0.0,0.0,0.0 +0,1.725800,0.0,0.0,0.0,0.0,0.0 +0,1.725900,0.0,0.0,0.0,0.0,0.0 +0,1.726000,0.0,0.0,0.0,0.0,0.0 +0,1.726100,0.0,0.0,0.0,0.0,0.0 +0,1.726200,0.0,0.0,0.0,0.0,0.0 +0,1.726300,0.0,0.0,0.0,0.0,0.0 +0,1.726400,0.0,0.0,0.0,0.0,0.0 +0,1.726500,0.0,0.0,0.0,0.0,0.0 +0,1.726600,0.0,0.0,0.0,0.0,0.0 +0,1.726700,0.0,0.0,0.0,0.0,0.0 +0,1.726800,0.0,0.0,0.0,0.0,0.0 +0,1.726900,0.0,0.0,0.0,0.0,0.0 +0,1.727000,0.0,0.0,0.0,0.0,0.0 +0,1.727100,0.0,0.0,0.0,0.0,0.0 +0,1.727200,0.0,0.0,0.0,0.0,0.0 +0,1.727300,0.0,0.0,0.0,0.0,0.0 +0,1.727400,0.0,0.0,0.0,0.0,0.0 +0,1.727500,0.0,0.0,0.0,0.0,0.0 +0,1.727600,0.0,0.0,0.0,0.0,0.0 +0,1.727700,0.0,0.0,0.0,0.0,0.0 +0,1.727800,0.0,0.0,0.0,0.0,0.0 +0,1.727900,0.0,0.0,0.0,0.0,0.0 +0,1.728000,0.0,0.0,0.0,0.0,0.0 +0,1.728100,0.0,0.0,0.0,0.0,0.0 +0,1.728200,0.0,0.0,0.0,0.0,0.0 +0,1.728300,0.0,0.0,0.0,0.0,0.0 +0,1.728400,0.0,0.0,0.0,0.0,0.0 +0,1.728500,0.0,0.0,0.0,0.0,0.0 +0,1.728600,0.0,0.0,0.0,0.0,0.0 +0,1.728700,0.0,0.0,0.0,0.0,0.0 +0,1.728800,0.0,0.0,0.0,0.0,0.0 +0,1.728900,0.0,0.0,0.0,0.0,0.0 +0,1.729000,0.0,0.0,0.0,0.0,0.0 +0,1.729100,0.0,0.0,0.0,0.0,0.0 +0,1.729200,0.0,0.0,0.0,0.0,0.0 +0,1.729300,0.0,0.0,0.0,0.0,0.0 +0,1.729400,0.0,0.0,0.0,0.0,0.0 +0,1.729500,0.0,0.0,0.0,0.0,0.0 +0,1.729600,0.0,0.0,0.0,0.0,0.0 +0,1.729700,0.0,0.0,0.0,0.0,0.0 +0,1.729800,0.0,0.0,0.0,0.0,0.0 +0,1.729900,0.0,0.0,0.0,0.0,0.0 +0,1.730000,0.0,0.0,0.0,0.0,0.0 +0,1.730100,0.0,0.0,0.0,0.0,0.0 +0,1.730200,0.0,0.0,0.0,0.0,0.0 +0,1.730300,0.0,0.0,0.0,0.0,0.0 +0,1.730400,0.0,0.0,0.0,0.0,0.0 +0,1.730500,0.0,0.0,0.0,0.0,0.0 +0,1.730600,0.0,0.0,0.0,0.0,0.0 +0,1.730700,0.0,0.0,0.0,0.0,0.0 +0,1.730800,0.0,0.0,0.0,0.0,0.0 +0,1.730900,0.0,0.0,0.0,0.0,0.0 +0,1.731000,0.0,0.0,0.0,0.0,0.0 +0,1.731100,0.0,0.0,0.0,0.0,0.0 +0,1.731200,0.0,0.0,0.0,0.0,0.0 +0,1.731300,0.0,0.0,0.0,0.0,0.0 +0,1.731400,0.0,0.0,0.0,0.0,0.0 +0,1.731500,0.0,0.0,0.0,0.0,0.0 +0,1.731600,0.0,0.0,0.0,0.0,0.0 +0,1.731700,0.0,0.0,0.0,0.0,0.0 +0,1.731800,0.0,0.0,0.0,0.0,0.0 +0,1.731900,0.0,0.0,0.0,0.0,0.0 +0,1.732000,0.0,0.0,0.0,0.0,0.0 +0,1.732100,0.0,0.0,0.0,0.0,0.0 +0,1.732200,0.0,0.0,0.0,0.0,0.0 +0,1.732300,0.0,0.0,0.0,0.0,0.0 +0,1.732400,0.0,0.0,0.0,0.0,0.0 +0,1.732500,0.0,0.0,0.0,0.0,0.0 +0,1.732600,0.0,0.0,0.0,0.0,0.0 +0,1.732700,0.0,0.0,0.0,0.0,0.0 +0,1.732800,0.0,0.0,0.0,0.0,0.0 +0,1.732900,0.0,0.0,0.0,0.0,0.0 +0,1.733000,0.0,0.0,0.0,0.0,0.0 +0,1.733100,0.0,0.0,0.0,0.0,0.0 +0,1.733200,0.0,0.0,0.0,0.0,0.0 +0,1.733300,0.0,0.0,0.0,0.0,0.0 +0,1.733400,0.0,0.0,0.0,0.0,0.0 +0,1.733500,0.0,0.0,0.0,0.0,0.0 +0,1.733600,0.0,0.0,0.0,0.0,0.0 +0,1.733700,0.0,0.0,0.0,0.0,0.0 +0,1.733800,0.0,0.0,0.0,0.0,0.0 +0,1.733900,0.0,0.0,0.0,0.0,0.0 +0,1.734000,0.0,0.0,0.0,0.0,0.0 +0,1.734100,0.0,0.0,0.0,0.0,0.0 +0,1.734200,0.0,0.0,0.0,0.0,0.0 +0,1.734300,0.0,0.0,0.0,0.0,0.0 +0,1.734400,0.0,0.0,0.0,0.0,0.0 +0,1.734500,0.0,0.0,0.0,0.0,0.0 +0,1.734600,0.0,0.0,0.0,0.0,0.0 +0,1.734700,0.0,0.0,0.0,0.0,0.0 +0,1.734800,0.0,0.0,0.0,0.0,0.0 +0,1.734900,0.0,0.0,0.0,0.0,0.0 +0,1.735000,0.0,0.0,0.0,0.0,0.0 +0,1.735100,0.0,0.0,0.0,0.0,0.0 +0,1.735200,0.0,0.0,0.0,0.0,0.0 +0,1.735300,0.0,0.0,0.0,0.0,0.0 +0,1.735400,0.0,0.0,0.0,0.0,0.0 +0,1.735500,0.0,0.0,0.0,0.0,0.0 +0,1.735600,0.0,0.0,0.0,0.0,0.0 +0,1.735700,0.0,0.0,0.0,0.0,0.0 +0,1.735800,0.0,0.0,0.0,0.0,0.0 +0,1.735900,0.0,0.0,0.0,0.0,0.0 +0,1.736000,0.0,0.0,0.0,0.0,0.0 +0,1.736100,0.0,0.0,0.0,0.0,0.0 +0,1.736200,0.0,0.0,0.0,0.0,0.0 +0,1.736300,0.0,0.0,0.0,0.0,0.0 +0,1.736400,0.0,0.0,0.0,0.0,0.0 +0,1.736500,0.0,0.0,0.0,0.0,0.0 +0,1.736600,0.0,0.0,0.0,0.0,0.0 +0,1.736700,0.0,0.0,0.0,0.0,0.0 +0,1.736800,0.0,0.0,0.0,0.0,0.0 +0,1.736900,0.0,0.0,0.0,0.0,0.0 +0,1.737000,0.0,0.0,0.0,0.0,0.0 +0,1.737100,0.0,0.0,0.0,0.0,0.0 +0,1.737200,0.0,0.0,0.0,0.0,0.0 +0,1.737300,0.0,0.0,0.0,0.0,0.0 +0,1.737400,0.0,0.0,0.0,0.0,0.0 +0,1.737500,0.0,0.0,0.0,0.0,0.0 +0,1.737600,0.0,0.0,0.0,0.0,0.0 +0,1.737700,0.0,0.0,0.0,0.0,0.0 +0,1.737800,0.0,0.0,0.0,0.0,0.0 +0,1.737900,0.0,0.0,0.0,0.0,0.0 +0,1.738000,0.0,0.0,0.0,0.0,0.0 +0,1.738100,0.0,0.0,0.0,0.0,0.0 +0,1.738200,0.0,0.0,0.0,0.0,0.0 +0,1.738300,0.0,0.0,0.0,0.0,0.0 +0,1.738400,0.0,0.0,0.0,0.0,0.0 +0,1.738500,0.0,0.0,0.0,0.0,0.0 +0,1.738600,0.0,0.0,0.0,0.0,0.0 +0,1.738700,0.0,0.0,0.0,0.0,0.0 +0,1.738800,0.0,0.0,0.0,0.0,0.0 +0,1.738900,0.0,0.0,0.0,0.0,0.0 +0,1.739000,0.0,0.0,0.0,0.0,0.0 +0,1.739100,0.0,0.0,0.0,0.0,0.0 +0,1.739200,0.0,0.0,0.0,0.0,0.0 +0,1.739300,0.0,0.0,0.0,0.0,0.0 +0,1.739400,0.0,0.0,0.0,0.0,0.0 +0,1.739500,0.0,0.0,0.0,0.0,0.0 +0,1.739600,0.0,0.0,0.0,0.0,0.0 +0,1.739700,0.0,0.0,0.0,0.0,0.0 +0,1.739800,0.0,0.0,0.0,0.0,0.0 +0,1.739900,0.0,0.0,0.0,0.0,0.0 +0,1.740000,0.0,0.0,0.0,0.0,0.0 +0,1.740100,0.0,0.0,0.0,0.0,0.0 +1,2195.577722,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.740200,0.0,0.0,0.0,0.0,0.0 +0,1.740300,0.0,0.0,0.0,0.0,0.0 +0,1.740400,0.0,0.0,0.0,0.0,0.0 +0,1.740500,0.0,0.0,0.0,0.0,0.0 +0,1.740600,0.0,0.0,0.0,0.0,0.0 +0,1.740700,0.0,0.0,0.0,0.0,0.0 +0,1.740800,0.0,0.0,0.0,0.0,0.0 +0,1.740900,0.0,0.0,0.0,0.0,0.0 +0,1.741000,0.0,0.0,0.0,0.0,0.0 +0,1.741100,0.0,0.0,0.0,0.0,0.0 +0,1.741200,0.0,0.0,0.0,0.0,0.0 +0,1.741300,0.0,0.0,0.0,0.0,0.0 +0,1.741400,0.0,0.0,0.0,0.0,0.0 +0,1.741500,0.0,0.0,0.0,0.0,0.0 +0,1.741600,0.0,0.0,0.0,0.0,0.0 +0,1.741700,0.0,0.0,0.0,0.0,0.0 +0,1.741800,0.0,0.0,0.0,0.0,0.0 +0,1.741900,0.0,0.0,0.0,0.0,0.0 +0,1.742000,0.0,0.0,0.0,0.0,0.0 +0,1.742100,0.0,0.0,0.0,0.0,0.0 +0,1.742200,0.0,0.0,0.0,0.0,0.0 +0,1.742300,0.0,0.0,0.0,0.0,0.0 +0,1.742400,0.0,0.0,0.0,0.0,0.0 +0,1.742500,0.0,0.0,0.0,0.0,0.0 +0,1.742600,0.0,0.0,0.0,0.0,0.0 +0,1.742700,0.0,0.0,0.0,0.0,0.0 +0,1.742800,0.0,0.0,0.0,0.0,0.0 +0,1.742900,0.0,0.0,0.0,0.0,0.0 +0,1.743000,0.0,0.0,0.0,0.0,0.0 +0,1.743100,0.0,0.0,0.0,0.0,0.0 +0,1.743200,0.0,0.0,0.0,0.0,0.0 +0,1.743300,0.0,0.0,0.0,0.0,0.0 +0,1.743400,0.0,0.0,0.0,0.0,0.0 +0,1.743500,0.0,0.0,0.0,0.0,0.0 +0,1.743600,0.0,0.0,0.0,0.0,0.0 +0,1.743700,0.0,0.0,0.0,0.0,0.0 +0,1.743800,0.0,0.0,0.0,0.0,0.0 +0,1.743900,0.0,0.0,0.0,0.0,0.0 +0,1.744000,0.0,0.0,0.0,0.0,0.0 +0,1.744100,0.0,0.0,0.0,0.0,0.0 +0,1.744200,0.0,0.0,0.0,0.0,0.0 +0,1.744300,0.0,0.0,0.0,0.0,0.0 +0,1.744400,0.0,0.0,0.0,0.0,0.0 +0,1.744500,0.0,0.0,0.0,0.0,0.0 +0,1.744600,0.0,0.0,0.0,0.0,0.0 +0,1.744700,0.0,0.0,0.0,0.0,0.0 +0,1.744800,0.0,0.0,0.0,0.0,0.0 +0,1.744900,0.0,0.0,0.0,0.0,0.0 +0,1.745000,0.0,0.0,0.0,0.0,0.0 +0,1.745100,0.0,0.0,0.0,0.0,0.0 +0,1.745200,0.0,0.0,0.0,0.0,0.0 +0,1.745300,0.0,0.0,0.0,0.0,0.0 +0,1.745400,0.0,0.0,0.0,0.0,0.0 +0,1.745500,0.0,0.0,0.0,0.0,0.0 +0,1.745600,0.0,0.0,0.0,0.0,0.0 +0,1.745700,0.0,0.0,0.0,0.0,0.0 +0,1.745800,0.0,0.0,0.0,0.0,0.0 +0,1.745900,0.0,0.0,0.0,0.0,0.0 +0,1.746000,0.0,0.0,0.0,0.0,0.0 +0,1.746100,0.0,0.0,0.0,0.0,0.0 +0,1.746200,0.0,0.0,0.0,0.0,0.0 +0,1.746300,0.0,0.0,0.0,0.0,0.0 +0,1.746400,0.0,0.0,0.0,0.0,0.0 +0,1.746500,0.0,0.0,0.0,0.0,0.0 +0,1.746600,0.0,0.0,0.0,0.0,0.0 +0,1.746700,0.0,0.0,0.0,0.0,0.0 +0,1.746800,0.0,0.0,0.0,0.0,0.0 +0,1.746900,0.0,0.0,0.0,0.0,0.0 +0,1.747000,0.0,0.0,0.0,0.0,0.0 +0,1.747100,0.0,0.0,0.0,0.0,0.0 +0,1.747200,0.0,0.0,0.0,0.0,0.0 +0,1.747300,0.0,0.0,0.0,0.0,0.0 +0,1.747400,0.0,0.0,0.0,0.0,0.0 +0,1.747500,0.0,0.0,0.0,0.0,0.0 +0,1.747600,0.0,0.0,0.0,0.0,0.0 +0,1.747700,0.0,0.0,0.0,0.0,0.0 +0,1.747800,0.0,0.0,0.0,0.0,0.0 +0,1.747900,0.0,0.0,0.0,0.0,0.0 +0,1.748000,0.0,0.0,0.0,0.0,0.0 +0,1.748100,0.0,0.0,0.0,0.0,0.0 +0,1.748200,0.0,0.0,0.0,0.0,0.0 +0,1.748300,0.0,0.0,0.0,0.0,0.0 +0,1.748400,0.0,0.0,0.0,0.0,0.0 +0,1.748500,0.0,0.0,0.0,0.0,0.0 +0,1.748600,0.0,0.0,0.0,0.0,0.0 +0,1.748700,0.0,0.0,0.0,0.0,0.0 +0,1.748800,0.0,0.0,0.0,0.0,0.0 +0,1.748900,0.0,0.0,0.0,0.0,0.0 +0,1.749000,0.0,0.0,0.0,0.0,0.0 +0,1.749100,0.0,0.0,0.0,0.0,0.0 +0,1.749200,0.0,0.0,0.0,0.0,0.0 +0,1.749300,0.0,0.0,0.0,0.0,0.0 +0,1.749400,0.0,0.0,0.0,0.0,0.0 +0,1.749500,0.0,0.0,0.0,0.0,0.0 +0,1.749600,0.0,0.0,0.0,0.0,0.0 +0,1.749700,0.0,0.0,0.0,0.0,0.0 +0,1.749800,0.0,0.0,0.0,0.0,0.0 +0,1.749900,0.0,0.0,0.0,0.0,0.0 +0,1.750000,0.0,0.0,0.0,0.0,0.0 +0,1.750100,0.0,0.0,0.0,0.0,0.0 +0,1.750200,0.0,0.0,0.0,0.0,0.0 +0,1.750300,0.0,0.0,0.0,0.0,0.0 +0,1.750400,0.0,0.0,0.0,0.0,0.0 +0,1.750500,0.0,0.0,0.0,0.0,0.0 +0,1.750600,0.0,0.0,0.0,0.0,0.0 +0,1.750700,0.0,0.0,0.0,0.0,0.0 +0,1.750800,0.0,0.0,0.0,0.0,0.0 +0,1.750900,0.0,0.0,0.0,0.0,0.0 +0,1.751000,0.0,0.0,0.0,0.0,0.0 +0,1.751100,0.0,0.0,0.0,0.0,0.0 +0,1.751200,0.0,0.0,0.0,0.0,0.0 +0,1.751300,0.0,0.0,0.0,0.0,0.0 +0,1.751400,0.0,0.0,0.0,0.0,0.0 +0,1.751500,0.0,0.0,0.0,0.0,0.0 +0,1.751600,0.0,0.0,0.0,0.0,0.0 +0,1.751700,0.0,0.0,0.0,0.0,0.0 +0,1.751800,0.0,0.0,0.0,0.0,0.0 +0,1.751900,0.0,0.0,0.0,0.0,0.0 +0,1.752000,0.0,0.0,0.0,0.0,0.0 +0,1.752100,0.0,0.0,0.0,0.0,0.0 +0,1.752200,0.0,0.0,0.0,0.0,0.0 +0,1.752300,0.0,0.0,0.0,0.0,0.0 +0,1.752400,0.0,0.0,0.0,0.0,0.0 +0,1.752500,0.0,0.0,0.0,0.0,0.0 +0,1.752600,0.0,0.0,0.0,0.0,0.0 +0,1.752700,0.0,0.0,0.0,0.0,0.0 +0,1.752800,0.0,0.0,0.0,0.0,0.0 +0,1.752900,0.0,0.0,0.0,0.0,0.0 +0,1.753000,0.0,0.0,0.0,0.0,0.0 +0,1.753100,0.0,0.0,0.0,0.0,0.0 +0,1.753200,0.0,0.0,0.0,0.0,0.0 +0,1.753300,0.0,0.0,0.0,0.0,0.0 +0,1.753400,0.0,0.0,0.0,0.0,0.0 +0,1.753500,0.0,0.0,0.0,0.0,0.0 +0,1.753600,0.0,0.0,0.0,0.0,0.0 +0,1.753700,0.0,0.0,0.0,0.0,0.0 +0,1.753800,0.0,0.0,0.0,0.0,0.0 +0,1.753900,0.0,0.0,0.0,0.0,0.0 +0,1.754000,0.0,0.0,0.0,0.0,0.0 +0,1.754100,0.0,0.0,0.0,0.0,0.0 +0,1.754200,0.0,0.0,0.0,0.0,0.0 +0,1.754300,0.0,0.0,0.0,0.0,0.0 +0,1.754400,0.0,0.0,0.0,0.0,0.0 +0,1.754500,0.0,0.0,0.0,0.0,0.0 +0,1.754600,0.0,0.0,0.0,0.0,0.0 +0,1.754700,0.0,0.0,0.0,0.0,0.0 +0,1.754800,0.0,0.0,0.0,0.0,0.0 +0,1.754900,0.0,0.0,0.0,0.0,0.0 +0,1.755000,0.0,0.0,0.0,0.0,0.0 +0,1.755100,0.0,0.0,0.0,0.0,0.0 +0,1.755200,0.0,0.0,0.0,0.0,0.0 +0,1.755300,0.0,0.0,0.0,0.0,0.0 +0,1.755400,0.0,0.0,0.0,0.0,0.0 +0,1.755500,0.0,0.0,0.0,0.0,0.0 +0,1.755600,0.0,0.0,0.0,0.0,0.0 +0,1.755700,0.0,0.0,0.0,0.0,0.0 +0,1.755800,0.0,0.0,0.0,0.0,0.0 +0,1.755900,0.0,0.0,0.0,0.0,0.0 +0,1.756000,0.0,0.0,0.0,0.0,0.0 +0,1.756100,0.0,0.0,0.0,0.0,0.0 +0,1.756200,0.0,0.0,0.0,0.0,0.0 +0,1.756300,0.0,0.0,0.0,0.0,0.0 +0,1.756400,0.0,0.0,0.0,0.0,0.0 +0,1.756500,0.0,0.0,0.0,0.0,0.0 +0,1.756600,0.0,0.0,0.0,0.0,0.0 +0,1.756700,0.0,0.0,0.0,0.0,0.0 +0,1.756800,0.0,0.0,0.0,0.0,0.0 +0,1.756900,0.0,0.0,0.0,0.0,0.0 +0,1.757000,0.0,0.0,0.0,0.0,0.0 +0,1.757100,0.0,0.0,0.0,0.0,0.0 +0,1.757200,0.0,0.0,0.0,0.0,0.0 +0,1.757300,0.0,0.0,0.0,0.0,0.0 +0,1.757400,0.0,0.0,0.0,0.0,0.0 +0,1.757500,0.0,0.0,0.0,0.0,0.0 +0,1.757600,0.0,0.0,0.0,0.0,0.0 +0,1.757700,0.0,0.0,0.0,0.0,0.0 +0,1.757800,0.0,0.0,0.0,0.0,0.0 +0,1.757900,0.0,0.0,0.0,0.0,0.0 +0,1.758000,0.0,0.0,0.0,0.0,0.0 +0,1.758100,0.0,0.0,0.0,0.0,0.0 +0,1.758200,0.0,0.0,0.0,0.0,0.0 +0,1.758300,0.0,0.0,0.0,0.0,0.0 +0,1.758400,0.0,0.0,0.0,0.0,0.0 +0,1.758500,0.0,0.0,0.0,0.0,0.0 +0,1.758600,0.0,0.0,0.0,0.0,0.0 +0,1.758700,0.0,0.0,0.0,0.0,0.0 +0,1.758800,0.0,0.0,0.0,0.0,0.0 +0,1.758900,0.0,0.0,0.0,0.0,0.0 +0,1.759000,0.0,0.0,0.0,0.0,0.0 +0,1.759100,0.0,0.0,0.0,0.0,0.0 +0,1.759200,0.0,0.0,0.0,0.0,0.0 +0,1.759300,0.0,0.0,0.0,0.0,0.0 +0,1.759400,0.0,0.0,0.0,0.0,0.0 +0,1.759500,0.0,0.0,0.0,0.0,0.0 +0,1.759600,0.0,0.0,0.0,0.0,0.0 +0,1.759700,0.0,0.0,0.0,0.0,0.0 +0,1.759800,0.0,0.0,0.0,0.0,0.0 +0,1.759900,0.0,0.0,0.0,0.0,0.0 +0,1.760000,0.0,0.0,0.0,0.0,0.0 +0,1.760100,0.0,0.0,0.0,0.0,0.0 +1,2272.154181,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.760200,0.0,0.0,0.0,0.0,0.0 +0,1.760300,0.0,0.0,0.0,0.0,0.0 +0,1.760400,0.0,0.0,0.0,0.0,0.0 +0,1.760500,0.0,0.0,0.0,0.0,0.0 +0,1.760600,0.0,0.0,0.0,0.0,0.0 +0,1.760700,0.0,0.0,0.0,0.0,0.0 +0,1.760800,0.0,0.0,0.0,0.0,0.0 +0,1.760900,0.0,0.0,0.0,0.0,0.0 +0,1.761000,0.0,0.0,0.0,0.0,0.0 +0,1.761100,0.0,0.0,0.0,0.0,0.0 +0,1.761200,0.0,0.0,0.0,0.0,0.0 +0,1.761300,0.0,0.0,0.0,0.0,0.0 +0,1.761400,0.0,0.0,0.0,0.0,0.0 +0,1.761500,0.0,0.0,0.0,0.0,0.0 +0,1.761600,0.0,0.0,0.0,0.0,0.0 +0,1.761700,0.0,0.0,0.0,0.0,0.0 +0,1.761800,0.0,0.0,0.0,0.0,0.0 +0,1.761900,0.0,0.0,0.0,0.0,0.0 +0,1.762000,0.0,0.0,0.0,0.0,0.0 +0,1.762100,0.0,0.0,0.0,0.0,0.0 +0,1.762200,0.0,0.0,0.0,0.0,0.0 +0,1.762300,0.0,0.0,0.0,0.0,0.0 +0,1.762400,0.0,0.0,0.0,0.0,0.0 +0,1.762500,0.0,0.0,0.0,0.0,0.0 +0,1.762600,0.0,0.0,0.0,0.0,0.0 +0,1.762700,0.0,0.0,0.0,0.0,0.0 +0,1.762800,0.0,0.0,0.0,0.0,0.0 +0,1.762900,0.0,0.0,0.0,0.0,0.0 +0,1.763000,0.0,0.0,0.0,0.0,0.0 +0,1.763100,0.0,0.0,0.0,0.0,0.0 +0,1.763200,0.0,0.0,0.0,0.0,0.0 +0,1.763300,0.0,0.0,0.0,0.0,0.0 +0,1.763400,0.0,0.0,0.0,0.0,0.0 +0,1.763500,0.0,0.0,0.0,0.0,0.0 +0,1.763600,0.0,0.0,0.0,0.0,0.0 +0,1.763700,0.0,0.0,0.0,0.0,0.0 +0,1.763800,0.0,0.0,0.0,0.0,0.0 +0,1.763900,0.0,0.0,0.0,0.0,0.0 +0,1.764000,0.0,0.0,0.0,0.0,0.0 +0,1.764100,0.0,0.0,0.0,0.0,0.0 +0,1.764200,0.0,0.0,0.0,0.0,0.0 +0,1.764300,0.0,0.0,0.0,0.0,0.0 +0,1.764400,0.0,0.0,0.0,0.0,0.0 +0,1.764500,0.0,0.0,0.0,0.0,0.0 +0,1.764600,0.0,0.0,0.0,0.0,0.0 +0,1.764700,0.0,0.0,0.0,0.0,0.0 +0,1.764800,0.0,0.0,0.0,0.0,0.0 +0,1.764900,0.0,0.0,0.0,0.0,0.0 +0,1.765000,0.0,0.0,0.0,0.0,0.0 +0,1.765100,0.0,0.0,0.0,0.0,0.0 +0,1.765200,0.0,0.0,0.0,0.0,0.0 +0,1.765300,0.0,0.0,0.0,0.0,0.0 +0,1.765400,0.0,0.0,0.0,0.0,0.0 +0,1.765500,0.0,0.0,0.0,0.0,0.0 +0,1.765600,0.0,0.0,0.0,0.0,0.0 +0,1.765700,0.0,0.0,0.0,0.0,0.0 +0,1.765800,0.0,0.0,0.0,0.0,0.0 +0,1.765900,0.0,0.0,0.0,0.0,0.0 +0,1.766000,0.0,0.0,0.0,0.0,0.0 +0,1.766100,0.0,0.0,0.0,0.0,0.0 +0,1.766200,0.0,0.0,0.0,0.0,0.0 +0,1.766300,0.0,0.0,0.0,0.0,0.0 +0,1.766400,0.0,0.0,0.0,0.0,0.0 +0,1.766500,0.0,0.0,0.0,0.0,0.0 +0,1.766600,0.0,0.0,0.0,0.0,0.0 +0,1.766700,0.0,0.0,0.0,0.0,0.0 +0,1.766800,0.0,0.0,0.0,0.0,0.0 +0,1.766900,0.0,0.0,0.0,0.0,0.0 +0,1.767000,0.0,0.0,0.0,0.0,0.0 +0,1.767100,0.0,0.0,0.0,0.0,0.0 +0,1.767200,0.0,0.0,0.0,0.0,0.0 +0,1.767300,0.0,0.0,0.0,0.0,0.0 +0,1.767400,0.0,0.0,0.0,0.0,0.0 +0,1.767500,0.0,0.0,0.0,0.0,0.0 +0,1.767600,0.0,0.0,0.0,0.0,0.0 +0,1.767700,0.0,0.0,0.0,0.0,0.0 +0,1.767800,0.0,0.0,0.0,0.0,0.0 +0,1.767900,0.0,0.0,0.0,0.0,0.0 +0,1.768000,0.0,0.0,0.0,0.0,0.0 +0,1.768100,0.0,0.0,0.0,0.0,0.0 +0,1.768200,0.0,0.0,0.0,0.0,0.0 +0,1.768300,0.0,0.0,0.0,0.0,0.0 +0,1.768400,0.0,0.0,0.0,0.0,0.0 +0,1.768500,0.0,0.0,0.0,0.0,0.0 +0,1.768600,0.0,0.0,0.0,0.0,0.0 +0,1.768700,0.0,0.0,0.0,0.0,0.0 +0,1.768800,0.0,0.0,0.0,0.0,0.0 +0,1.768900,0.0,0.0,0.0,0.0,0.0 +0,1.769000,0.0,0.0,0.0,0.0,0.0 +0,1.769100,0.0,0.0,0.0,0.0,0.0 +0,1.769200,0.0,0.0,0.0,0.0,0.0 +0,1.769300,0.0,0.0,0.0,0.0,0.0 +0,1.769400,0.0,0.0,0.0,0.0,0.0 +0,1.769500,0.0,0.0,0.0,0.0,0.0 +0,1.769600,0.0,0.0,0.0,0.0,0.0 +0,1.769700,0.0,0.0,0.0,0.0,0.0 +0,1.769800,0.0,0.0,0.0,0.0,0.0 +0,1.769900,0.0,0.0,0.0,0.0,0.0 +0,1.770000,0.0,0.0,0.0,0.0,0.0 +0,1.770100,0.0,0.0,0.0,0.0,0.0 +0,1.770200,0.0,0.0,0.0,0.0,0.0 +0,1.770300,0.0,0.0,0.0,0.0,0.0 +0,1.770400,0.0,0.0,0.0,0.0,0.0 +0,1.770500,0.0,0.0,0.0,0.0,0.0 +0,1.770600,0.0,0.0,0.0,0.0,0.0 +0,1.770700,0.0,0.0,0.0,0.0,0.0 +0,1.770800,0.0,0.0,0.0,0.0,0.0 +0,1.770900,0.0,0.0,0.0,0.0,0.0 +0,1.771000,0.0,0.0,0.0,0.0,0.0 +0,1.771100,0.0,0.0,0.0,0.0,0.0 +0,1.771200,0.0,0.0,0.0,0.0,0.0 +0,1.771300,0.0,0.0,0.0,0.0,0.0 +0,1.771400,0.0,0.0,0.0,0.0,0.0 +0,1.771500,0.0,0.0,0.0,0.0,0.0 +0,1.771600,0.0,0.0,0.0,0.0,0.0 +0,1.771700,0.0,0.0,0.0,0.0,0.0 +0,1.771800,0.0,0.0,0.0,0.0,0.0 +0,1.771900,0.0,0.0,0.0,0.0,0.0 +0,1.772000,0.0,0.0,0.0,0.0,0.0 +0,1.772100,0.0,0.0,0.0,0.0,0.0 +0,1.772200,0.0,0.0,0.0,0.0,0.0 +0,1.772300,0.0,0.0,0.0,0.0,0.0 +0,1.772400,0.0,0.0,0.0,0.0,0.0 +0,1.772500,0.0,0.0,0.0,0.0,0.0 +0,1.772600,0.0,0.0,0.0,0.0,0.0 +0,1.772700,0.0,0.0,0.0,0.0,0.0 +0,1.772800,0.0,0.0,0.0,0.0,0.0 +0,1.772900,0.0,0.0,0.0,0.0,0.0 +0,1.773000,0.0,0.0,0.0,0.0,0.0 +0,1.773100,0.0,0.0,0.0,0.0,0.0 +0,1.773200,0.0,0.0,0.0,0.0,0.0 +0,1.773300,0.0,0.0,0.0,0.0,0.0 +0,1.773400,0.0,0.0,0.0,0.0,0.0 +0,1.773500,0.0,0.0,0.0,0.0,0.0 +0,1.773600,0.0,0.0,0.0,0.0,0.0 +0,1.773700,0.0,0.0,0.0,0.0,0.0 +0,1.773800,0.0,0.0,0.0,0.0,0.0 +0,1.773900,0.0,0.0,0.0,0.0,0.0 +0,1.774000,0.0,0.0,0.0,0.0,0.0 +0,1.774100,0.0,0.0,0.0,0.0,0.0 +0,1.774200,0.0,0.0,0.0,0.0,0.0 +0,1.774300,0.0,0.0,0.0,0.0,0.0 +0,1.774400,0.0,0.0,0.0,0.0,0.0 +0,1.774500,0.0,0.0,0.0,0.0,0.0 +0,1.774600,0.0,0.0,0.0,0.0,0.0 +0,1.774700,0.0,0.0,0.0,0.0,0.0 +0,1.774800,0.0,0.0,0.0,0.0,0.0 +0,1.774900,0.0,0.0,0.0,0.0,0.0 +0,1.775000,0.0,0.0,0.0,0.0,0.0 +0,1.775100,0.0,0.0,0.0,0.0,0.0 +0,1.775200,0.0,0.0,0.0,0.0,0.0 +0,1.775300,0.0,0.0,0.0,0.0,0.0 +0,1.775400,0.0,0.0,0.0,0.0,0.0 +0,1.775500,0.0,0.0,0.0,0.0,0.0 +0,1.775600,0.0,0.0,0.0,0.0,0.0 +0,1.775700,0.0,0.0,0.0,0.0,0.0 +0,1.775800,0.0,0.0,0.0,0.0,0.0 +0,1.775900,0.0,0.0,0.0,0.0,0.0 +0,1.776000,0.0,0.0,0.0,0.0,0.0 +0,1.776100,0.0,0.0,0.0,0.0,0.0 +0,1.776200,0.0,0.0,0.0,0.0,0.0 +0,1.776300,0.0,0.0,0.0,0.0,0.0 +0,1.776400,0.0,0.0,0.0,0.0,0.0 +0,1.776500,0.0,0.0,0.0,0.0,0.0 +0,1.776600,0.0,0.0,0.0,0.0,0.0 +0,1.776700,0.0,0.0,0.0,0.0,0.0 +0,1.776800,0.0,0.0,0.0,0.0,0.0 +0,1.776900,0.0,0.0,0.0,0.0,0.0 +0,1.777000,0.0,0.0,0.0,0.0,0.0 +0,1.777100,0.0,0.0,0.0,0.0,0.0 +0,1.777200,0.0,0.0,0.0,0.0,0.0 +0,1.777300,0.0,0.0,0.0,0.0,0.0 +0,1.777400,0.0,0.0,0.0,0.0,0.0 +0,1.777500,0.0,0.0,0.0,0.0,0.0 +0,1.777600,0.0,0.0,0.0,0.0,0.0 +0,1.777700,0.0,0.0,0.0,0.0,0.0 +0,1.777800,0.0,0.0,0.0,0.0,0.0 +0,1.777900,0.0,0.0,0.0,0.0,0.0 +0,1.778000,0.0,0.0,0.0,0.0,0.0 +0,1.778100,0.0,0.0,0.0,0.0,0.0 +0,1.778200,0.0,0.0,0.0,0.0,0.0 +0,1.778300,0.0,0.0,0.0,0.0,0.0 +0,1.778400,0.0,0.0,0.0,0.0,0.0 +0,1.778500,0.0,0.0,0.0,0.0,0.0 +0,1.778600,0.0,0.0,0.0,0.0,0.0 +0,1.778700,0.0,0.0,0.0,0.0,0.0 +0,1.778800,0.0,0.0,0.0,0.0,0.0 +0,1.778900,0.0,0.0,0.0,0.0,0.0 +0,1.779000,0.0,0.0,0.0,0.0,0.0 +0,1.779100,0.0,0.0,0.0,0.0,0.0 +0,1.779200,0.0,0.0,0.0,0.0,0.0 +0,1.779300,0.0,0.0,0.0,0.0,0.0 +0,1.779400,0.0,0.0,0.0,0.0,0.0 +0,1.779500,0.0,0.0,0.0,0.0,0.0 +0,1.779600,0.0,0.0,0.0,0.0,0.0 +0,1.779700,0.0,0.0,0.0,0.0,0.0 +0,1.779800,0.0,0.0,0.0,0.0,0.0 +0,1.779900,0.0,0.0,0.0,0.0,0.0 +0,1.780000,0.0,0.0,0.0,0.0,0.0 +0,1.780100,0.0,0.0,0.0,0.0,0.0 +1,2350.490790,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.780200,0.0,0.0,0.0,0.0,0.0 +0,1.780300,0.0,0.0,0.0,0.0,0.0 +0,1.780400,0.0,0.0,0.0,0.0,0.0 +0,1.780500,0.0,0.0,0.0,0.0,0.0 +0,1.780600,0.0,0.0,0.0,0.0,0.0 +0,1.780700,0.0,0.0,0.0,0.0,0.0 +0,1.780800,0.0,0.0,0.0,0.0,0.0 +0,1.780900,0.0,0.0,0.0,0.0,0.0 +0,1.781000,0.0,0.0,0.0,0.0,0.0 +0,1.781100,0.0,0.0,0.0,0.0,0.0 +0,1.781200,0.0,0.0,0.0,0.0,0.0 +0,1.781300,0.0,0.0,0.0,0.0,0.0 +0,1.781400,0.0,0.0,0.0,0.0,0.0 +0,1.781500,0.0,0.0,0.0,0.0,0.0 +0,1.781600,0.0,0.0,0.0,0.0,0.0 +0,1.781700,0.0,0.0,0.0,0.0,0.0 +0,1.781800,0.0,0.0,0.0,0.0,0.0 +0,1.781900,0.0,0.0,0.0,0.0,0.0 +0,1.782000,0.0,0.0,0.0,0.0,0.0 +0,1.782100,0.0,0.0,0.0,0.0,0.0 +0,1.782200,0.0,0.0,0.0,0.0,0.0 +0,1.782300,0.0,0.0,0.0,0.0,0.0 +0,1.782400,0.0,0.0,0.0,0.0,0.0 +0,1.782500,0.0,0.0,0.0,0.0,0.0 +0,1.782600,0.0,0.0,0.0,0.0,0.0 +0,1.782700,0.0,0.0,0.0,0.0,0.0 +0,1.782800,0.0,0.0,0.0,0.0,0.0 +0,1.782900,0.0,0.0,0.0,0.0,0.0 +0,1.783000,0.0,0.0,0.0,0.0,0.0 +0,1.783100,0.0,0.0,0.0,0.0,0.0 +0,1.783200,0.0,0.0,0.0,0.0,0.0 +0,1.783300,0.0,0.0,0.0,0.0,0.0 +0,1.783400,0.0,0.0,0.0,0.0,0.0 +0,1.783500,0.0,0.0,0.0,0.0,0.0 +0,1.783600,0.0,0.0,0.0,0.0,0.0 +0,1.783700,0.0,0.0,0.0,0.0,0.0 +0,1.783800,0.0,0.0,0.0,0.0,0.0 +0,1.783900,0.0,0.0,0.0,0.0,0.0 +0,1.784000,0.0,0.0,0.0,0.0,0.0 +0,1.784100,0.0,0.0,0.0,0.0,0.0 +0,1.784200,0.0,0.0,0.0,0.0,0.0 +0,1.784300,0.0,0.0,0.0,0.0,0.0 +0,1.784400,0.0,0.0,0.0,0.0,0.0 +0,1.784500,0.0,0.0,0.0,0.0,0.0 +0,1.784600,0.0,0.0,0.0,0.0,0.0 +0,1.784700,0.0,0.0,0.0,0.0,0.0 +0,1.784800,0.0,0.0,0.0,0.0,0.0 +0,1.784900,0.0,0.0,0.0,0.0,0.0 +0,1.785000,0.0,0.0,0.0,0.0,0.0 +0,1.785100,0.0,0.0,0.0,0.0,0.0 +0,1.785200,0.0,0.0,0.0,0.0,0.0 +0,1.785300,0.0,0.0,0.0,0.0,0.0 +0,1.785400,0.0,0.0,0.0,0.0,0.0 +0,1.785500,0.0,0.0,0.0,0.0,0.0 +0,1.785600,0.0,0.0,0.0,0.0,0.0 +0,1.785700,0.0,0.0,0.0,0.0,0.0 +0,1.785800,0.0,0.0,0.0,0.0,0.0 +0,1.785900,0.0,0.0,0.0,0.0,0.0 +0,1.786000,0.0,0.0,0.0,0.0,0.0 +0,1.786100,0.0,0.0,0.0,0.0,0.0 +0,1.786200,0.0,0.0,0.0,0.0,0.0 +0,1.786300,0.0,0.0,0.0,0.0,0.0 +0,1.786400,0.0,0.0,0.0,0.0,0.0 +0,1.786500,0.0,0.0,0.0,0.0,0.0 +0,1.786600,0.0,0.0,0.0,0.0,0.0 +0,1.786700,0.0,0.0,0.0,0.0,0.0 +0,1.786800,0.0,0.0,0.0,0.0,0.0 +0,1.786900,0.0,0.0,0.0,0.0,0.0 +0,1.787000,0.0,0.0,0.0,0.0,0.0 +0,1.787100,0.0,0.0,0.0,0.0,0.0 +0,1.787200,0.0,0.0,0.0,0.0,0.0 +0,1.787300,0.0,0.0,0.0,0.0,0.0 +0,1.787400,0.0,0.0,0.0,0.0,0.0 +0,1.787500,0.0,0.0,0.0,0.0,0.0 +0,1.787600,0.0,0.0,0.0,0.0,0.0 +0,1.787700,0.0,0.0,0.0,0.0,0.0 +0,1.787800,0.0,0.0,0.0,0.0,0.0 +0,1.787900,0.0,0.0,0.0,0.0,0.0 +0,1.788000,0.0,0.0,0.0,0.0,0.0 +0,1.788100,0.0,0.0,0.0,0.0,0.0 +0,1.788200,0.0,0.0,0.0,0.0,0.0 +0,1.788300,0.0,0.0,0.0,0.0,0.0 +0,1.788400,0.0,0.0,0.0,0.0,0.0 +0,1.788500,0.0,0.0,0.0,0.0,0.0 +0,1.788600,0.0,0.0,0.0,0.0,0.0 +0,1.788700,0.0,0.0,0.0,0.0,0.0 +0,1.788800,0.0,0.0,0.0,0.0,0.0 +0,1.788900,0.0,0.0,0.0,0.0,0.0 +0,1.789000,0.0,0.0,0.0,0.0,0.0 +0,1.789100,0.0,0.0,0.0,0.0,0.0 +0,1.789200,0.0,0.0,0.0,0.0,0.0 +0,1.789300,0.0,0.0,0.0,0.0,0.0 +0,1.789400,0.0,0.0,0.0,0.0,0.0 +0,1.789500,0.0,0.0,0.0,0.0,0.0 +0,1.789600,0.0,0.0,0.0,0.0,0.0 +0,1.789700,0.0,0.0,0.0,0.0,0.0 +0,1.789800,0.0,0.0,0.0,0.0,0.0 +0,1.789900,0.0,0.0,0.0,0.0,0.0 +0,1.790000,0.0,0.0,0.0,0.0,0.0 +0,1.790100,0.0,0.0,0.0,0.0,0.0 +0,1.790200,0.0,0.0,0.0,0.0,0.0 +0,1.790300,0.0,0.0,0.0,0.0,0.0 +0,1.790400,0.0,0.0,0.0,0.0,0.0 +0,1.790500,0.0,0.0,0.0,0.0,0.0 +0,1.790600,0.0,0.0,0.0,0.0,0.0 +0,1.790700,0.0,0.0,0.0,0.0,0.0 +0,1.790800,0.0,0.0,0.0,0.0,0.0 +0,1.790900,0.0,0.0,0.0,0.0,0.0 +0,1.791000,0.0,0.0,0.0,0.0,0.0 +0,1.791100,0.0,0.0,0.0,0.0,0.0 +0,1.791200,0.0,0.0,0.0,0.0,0.0 +0,1.791300,0.0,0.0,0.0,0.0,0.0 +0,1.791400,0.0,0.0,0.0,0.0,0.0 +0,1.791500,0.0,0.0,0.0,0.0,0.0 +0,1.791600,0.0,0.0,0.0,0.0,0.0 +0,1.791700,0.0,0.0,0.0,0.0,0.0 +0,1.791800,0.0,0.0,0.0,0.0,0.0 +0,1.791900,0.0,0.0,0.0,0.0,0.0 +0,1.792000,0.0,0.0,0.0,0.0,0.0 +0,1.792100,0.0,0.0,0.0,0.0,0.0 +0,1.792200,0.0,0.0,0.0,0.0,0.0 +0,1.792300,0.0,0.0,0.0,0.0,0.0 +0,1.792400,0.0,0.0,0.0,0.0,0.0 +0,1.792500,0.0,0.0,0.0,0.0,0.0 +0,1.792600,0.0,0.0,0.0,0.0,0.0 +0,1.792700,0.0,0.0,0.0,0.0,0.0 +0,1.792800,0.0,0.0,0.0,0.0,0.0 +0,1.792900,0.0,0.0,0.0,0.0,0.0 +0,1.793000,0.0,0.0,0.0,0.0,0.0 +0,1.793100,0.0,0.0,0.0,0.0,0.0 +0,1.793200,0.0,0.0,0.0,0.0,0.0 +0,1.793300,0.0,0.0,0.0,0.0,0.0 +0,1.793400,0.0,0.0,0.0,0.0,0.0 +0,1.793500,0.0,0.0,0.0,0.0,0.0 +0,1.793600,0.0,0.0,0.0,0.0,0.0 +0,1.793700,0.0,0.0,0.0,0.0,0.0 +0,1.793800,0.0,0.0,0.0,0.0,0.0 +0,1.793900,0.0,0.0,0.0,0.0,0.0 +0,1.794000,0.0,0.0,0.0,0.0,0.0 +0,1.794100,0.0,0.0,0.0,0.0,0.0 +0,1.794200,0.0,0.0,0.0,0.0,0.0 +0,1.794300,0.0,0.0,0.0,0.0,0.0 +0,1.794400,0.0,0.0,0.0,0.0,0.0 +0,1.794500,0.0,0.0,0.0,0.0,0.0 +0,1.794600,0.0,0.0,0.0,0.0,0.0 +0,1.794700,0.0,0.0,0.0,0.0,0.0 +0,1.794800,0.0,0.0,0.0,0.0,0.0 +0,1.794900,0.0,0.0,0.0,0.0,0.0 +0,1.795000,0.0,0.0,0.0,0.0,0.0 +0,1.795100,0.0,0.0,0.0,0.0,0.0 +0,1.795200,0.0,0.0,0.0,0.0,0.0 +0,1.795300,0.0,0.0,0.0,0.0,0.0 +0,1.795400,0.0,0.0,0.0,0.0,0.0 +0,1.795500,0.0,0.0,0.0,0.0,0.0 +0,1.795600,0.0,0.0,0.0,0.0,0.0 +0,1.795700,0.0,0.0,0.0,0.0,0.0 +0,1.795800,0.0,0.0,0.0,0.0,0.0 +0,1.795900,0.0,0.0,0.0,0.0,0.0 +0,1.796000,0.0,0.0,0.0,0.0,0.0 +0,1.796100,0.0,0.0,0.0,0.0,0.0 +0,1.796200,0.0,0.0,0.0,0.0,0.0 +0,1.796300,0.0,0.0,0.0,0.0,0.0 +0,1.796400,0.0,0.0,0.0,0.0,0.0 +0,1.796500,0.0,0.0,0.0,0.0,0.0 +0,1.796600,0.0,0.0,0.0,0.0,0.0 +0,1.796700,0.0,0.0,0.0,0.0,0.0 +0,1.796800,0.0,0.0,0.0,0.0,0.0 +0,1.796900,0.0,0.0,0.0,0.0,0.0 +0,1.797000,0.0,0.0,0.0,0.0,0.0 +0,1.797100,0.0,0.0,0.0,0.0,0.0 +0,1.797200,0.0,0.0,0.0,0.0,0.0 +0,1.797300,0.0,0.0,0.0,0.0,0.0 +0,1.797400,0.0,0.0,0.0,0.0,0.0 +0,1.797500,0.0,0.0,0.0,0.0,0.0 +0,1.797600,0.0,0.0,0.0,0.0,0.0 +0,1.797700,0.0,0.0,0.0,0.0,0.0 +0,1.797800,0.0,0.0,0.0,0.0,0.0 +0,1.797900,0.0,0.0,0.0,0.0,0.0 +0,1.798000,0.0,0.0,0.0,0.0,0.0 +0,1.798100,0.0,0.0,0.0,0.0,0.0 +0,1.798200,0.0,0.0,0.0,0.0,0.0 +0,1.798300,0.0,0.0,0.0,0.0,0.0 +0,1.798400,0.0,0.0,0.0,0.0,0.0 +0,1.798500,0.0,0.0,0.0,0.0,0.0 +0,1.798600,0.0,0.0,0.0,0.0,0.0 +0,1.798700,0.0,0.0,0.0,0.0,0.0 +0,1.798800,0.0,0.0,0.0,0.0,0.0 +0,1.798900,0.0,0.0,0.0,0.0,0.0 +0,1.799000,0.0,0.0,0.0,0.0,0.0 +0,1.799100,0.0,0.0,0.0,0.0,0.0 +0,1.799200,0.0,0.0,0.0,0.0,0.0 +0,1.799300,0.0,0.0,0.0,0.0,0.0 +0,1.799400,0.0,0.0,0.0,0.0,0.0 +0,1.799500,0.0,0.0,0.0,0.0,0.0 +0,1.799600,0.0,0.0,0.0,0.0,0.0 +0,1.799700,0.0,0.0,0.0,0.0,0.0 +0,1.799800,0.0,0.0,0.0,0.0,0.0 +0,1.799900,0.0,0.0,0.0,0.0,0.0 +0,1.800000,0.0,0.0,0.0,0.0,0.0 +0,1.800100,0.0,0.0,0.0,0.0,0.0 +1,2430.607549,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.800200,0.0,0.0,0.0,0.0,0.0 +0,1.800300,0.0,0.0,0.0,0.0,0.0 +0,1.800400,0.0,0.0,0.0,0.0,0.0 +0,1.800500,0.0,0.0,0.0,0.0,0.0 +0,1.800600,0.0,0.0,0.0,0.0,0.0 +0,1.800700,0.0,0.0,0.0,0.0,0.0 +0,1.800800,0.0,0.0,0.0,0.0,0.0 +0,1.800900,0.0,0.0,0.0,0.0,0.0 +0,1.801000,0.0,0.0,0.0,0.0,0.0 +0,1.801100,0.0,0.0,0.0,0.0,0.0 +0,1.801200,0.0,0.0,0.0,0.0,0.0 +0,1.801300,0.0,0.0,0.0,0.0,0.0 +0,1.801400,0.0,0.0,0.0,0.0,0.0 +0,1.801500,0.0,0.0,0.0,0.0,0.0 +0,1.801600,0.0,0.0,0.0,0.0,0.0 +0,1.801700,0.0,0.0,0.0,0.0,0.0 +0,1.801800,0.0,0.0,0.0,0.0,0.0 +0,1.801900,0.0,0.0,0.0,0.0,0.0 +0,1.802000,0.0,0.0,0.0,0.0,0.0 +0,1.802100,0.0,0.0,0.0,0.0,0.0 +0,1.802200,0.0,0.0,0.0,0.0,0.0 +0,1.802300,0.0,0.0,0.0,0.0,0.0 +0,1.802400,0.0,0.0,0.0,0.0,0.0 +0,1.802500,0.0,0.0,0.0,0.0,0.0 +0,1.802600,0.0,0.0,0.0,0.0,0.0 +0,1.802700,0.0,0.0,0.0,0.0,0.0 +0,1.802800,0.0,0.0,0.0,0.0,0.0 +0,1.802900,0.0,0.0,0.0,0.0,0.0 +0,1.803000,0.0,0.0,0.0,0.0,0.0 +0,1.803100,0.0,0.0,0.0,0.0,0.0 +0,1.803200,0.0,0.0,0.0,0.0,0.0 +0,1.803300,0.0,0.0,0.0,0.0,0.0 +0,1.803400,0.0,0.0,0.0,0.0,0.0 +0,1.803500,0.0,0.0,0.0,0.0,0.0 +0,1.803600,0.0,0.0,0.0,0.0,0.0 +0,1.803700,0.0,0.0,0.0,0.0,0.0 +0,1.803800,0.0,0.0,0.0,0.0,0.0 +0,1.803900,0.0,0.0,0.0,0.0,0.0 +0,1.804000,0.0,0.0,0.0,0.0,0.0 +0,1.804100,0.0,0.0,0.0,0.0,0.0 +0,1.804200,0.0,0.0,0.0,0.0,0.0 +0,1.804300,0.0,0.0,0.0,0.0,0.0 +0,1.804400,0.0,0.0,0.0,0.0,0.0 +0,1.804500,0.0,0.0,0.0,0.0,0.0 +0,1.804600,0.0,0.0,0.0,0.0,0.0 +0,1.804700,0.0,0.0,0.0,0.0,0.0 +0,1.804800,0.0,0.0,0.0,0.0,0.0 +0,1.804900,0.0,0.0,0.0,0.0,0.0 +0,1.805000,0.0,0.0,0.0,0.0,0.0 +0,1.805100,0.0,0.0,0.0,0.0,0.0 +0,1.805200,0.0,0.0,0.0,0.0,0.0 +0,1.805300,0.0,0.0,0.0,0.0,0.0 +0,1.805400,0.0,0.0,0.0,0.0,0.0 +0,1.805500,0.0,0.0,0.0,0.0,0.0 +0,1.805600,0.0,0.0,0.0,0.0,0.0 +0,1.805700,0.0,0.0,0.0,0.0,0.0 +0,1.805800,0.0,0.0,0.0,0.0,0.0 +0,1.805900,0.0,0.0,0.0,0.0,0.0 +0,1.806000,0.0,0.0,0.0,0.0,0.0 +0,1.806100,0.0,0.0,0.0,0.0,0.0 +0,1.806200,0.0,0.0,0.0,0.0,0.0 +0,1.806300,0.0,0.0,0.0,0.0,0.0 +0,1.806400,0.0,0.0,0.0,0.0,0.0 +0,1.806500,0.0,0.0,0.0,0.0,0.0 +0,1.806600,0.0,0.0,0.0,0.0,0.0 +0,1.806700,0.0,0.0,0.0,0.0,0.0 +0,1.806800,0.0,0.0,0.0,0.0,0.0 +0,1.806900,0.0,0.0,0.0,0.0,0.0 +0,1.807000,0.0,0.0,0.0,0.0,0.0 +0,1.807100,0.0,0.0,0.0,0.0,0.0 +0,1.807200,0.0,0.0,0.0,0.0,0.0 +0,1.807300,0.0,0.0,0.0,0.0,0.0 +0,1.807400,0.0,0.0,0.0,0.0,0.0 +0,1.807500,0.0,0.0,0.0,0.0,0.0 +0,1.807600,0.0,0.0,0.0,0.0,0.0 +0,1.807700,0.0,0.0,0.0,0.0,0.0 +0,1.807800,0.0,0.0,0.0,0.0,0.0 +0,1.807900,0.0,0.0,0.0,0.0,0.0 +0,1.808000,0.0,0.0,0.0,0.0,0.0 +0,1.808100,0.0,0.0,0.0,0.0,0.0 +0,1.808200,0.0,0.0,0.0,0.0,0.0 +0,1.808300,0.0,0.0,0.0,0.0,0.0 +0,1.808400,0.0,0.0,0.0,0.0,0.0 +0,1.808500,0.0,0.0,0.0,0.0,0.0 +0,1.808600,0.0,0.0,0.0,0.0,0.0 +0,1.808700,0.0,0.0,0.0,0.0,0.0 +0,1.808800,0.0,0.0,0.0,0.0,0.0 +0,1.808900,0.0,0.0,0.0,0.0,0.0 +0,1.809000,0.0,0.0,0.0,0.0,0.0 +0,1.809100,0.0,0.0,0.0,0.0,0.0 +0,1.809200,0.0,0.0,0.0,0.0,0.0 +0,1.809300,0.0,0.0,0.0,0.0,0.0 +0,1.809400,0.0,0.0,0.0,0.0,0.0 +0,1.809500,0.0,0.0,0.0,0.0,0.0 +0,1.809600,0.0,0.0,0.0,0.0,0.0 +0,1.809700,0.0,0.0,0.0,0.0,0.0 +0,1.809800,0.0,0.0,0.0,0.0,0.0 +0,1.809900,0.0,0.0,0.0,0.0,0.0 +0,1.810000,0.0,0.0,0.0,0.0,0.0 +0,1.810100,0.0,0.0,0.0,0.0,0.0 +0,1.810200,0.0,0.0,0.0,0.0,0.0 +0,1.810300,0.0,0.0,0.0,0.0,0.0 +0,1.810400,0.0,0.0,0.0,0.0,0.0 +0,1.810500,0.0,0.0,0.0,0.0,0.0 +0,1.810600,0.0,0.0,0.0,0.0,0.0 +0,1.810700,0.0,0.0,0.0,0.0,0.0 +0,1.810800,0.0,0.0,0.0,0.0,0.0 +0,1.810900,0.0,0.0,0.0,0.0,0.0 +0,1.811000,0.0,0.0,0.0,0.0,0.0 +0,1.811100,0.0,0.0,0.0,0.0,0.0 +0,1.811200,0.0,0.0,0.0,0.0,0.0 +0,1.811300,0.0,0.0,0.0,0.0,0.0 +0,1.811400,0.0,0.0,0.0,0.0,0.0 +0,1.811500,0.0,0.0,0.0,0.0,0.0 +0,1.811600,0.0,0.0,0.0,0.0,0.0 +0,1.811700,0.0,0.0,0.0,0.0,0.0 +0,1.811800,0.0,0.0,0.0,0.0,0.0 +0,1.811900,0.0,0.0,0.0,0.0,0.0 +0,1.812000,0.0,0.0,0.0,0.0,0.0 +0,1.812100,0.0,0.0,0.0,0.0,0.0 +0,1.812200,0.0,0.0,0.0,0.0,0.0 +0,1.812300,0.0,0.0,0.0,0.0,0.0 +0,1.812400,0.0,0.0,0.0,0.0,0.0 +0,1.812500,0.0,0.0,0.0,0.0,0.0 +0,1.812600,0.0,0.0,0.0,0.0,0.0 +0,1.812700,0.0,0.0,0.0,0.0,0.0 +0,1.812800,0.0,0.0,0.0,0.0,0.0 +0,1.812900,0.0,0.0,0.0,0.0,0.0 +0,1.813000,0.0,0.0,0.0,0.0,0.0 +0,1.813100,0.0,0.0,0.0,0.0,0.0 +0,1.813200,0.0,0.0,0.0,0.0,0.0 +0,1.813300,0.0,0.0,0.0,0.0,0.0 +0,1.813400,0.0,0.0,0.0,0.0,0.0 +0,1.813500,0.0,0.0,0.0,0.0,0.0 +0,1.813600,0.0,0.0,0.0,0.0,0.0 +0,1.813700,0.0,0.0,0.0,0.0,0.0 +0,1.813800,0.0,0.0,0.0,0.0,0.0 +0,1.813900,0.0,0.0,0.0,0.0,0.0 +0,1.814000,0.0,0.0,0.0,0.0,0.0 +0,1.814100,0.0,0.0,0.0,0.0,0.0 +0,1.814200,0.0,0.0,0.0,0.0,0.0 +0,1.814300,0.0,0.0,0.0,0.0,0.0 +0,1.814400,0.0,0.0,0.0,0.0,0.0 +0,1.814500,0.0,0.0,0.0,0.0,0.0 +0,1.814600,0.0,0.0,0.0,0.0,0.0 +0,1.814700,0.0,0.0,0.0,0.0,0.0 +0,1.814800,0.0,0.0,0.0,0.0,0.0 +0,1.814900,0.0,0.0,0.0,0.0,0.0 +0,1.815000,0.0,0.0,0.0,0.0,0.0 +0,1.815100,0.0,0.0,0.0,0.0,0.0 +0,1.815200,0.0,0.0,0.0,0.0,0.0 +0,1.815300,0.0,0.0,0.0,0.0,0.0 +0,1.815400,0.0,0.0,0.0,0.0,0.0 +0,1.815500,0.0,0.0,0.0,0.0,0.0 +0,1.815600,0.0,0.0,0.0,0.0,0.0 +0,1.815700,0.0,0.0,0.0,0.0,0.0 +0,1.815800,0.0,0.0,0.0,0.0,0.0 +0,1.815900,0.0,0.0,0.0,0.0,0.0 +0,1.816000,0.0,0.0,0.0,0.0,0.0 +0,1.816100,0.0,0.0,0.0,0.0,0.0 +0,1.816200,0.0,0.0,0.0,0.0,0.0 +0,1.816300,0.0,0.0,0.0,0.0,0.0 +0,1.816400,0.0,0.0,0.0,0.0,0.0 +0,1.816500,0.0,0.0,0.0,0.0,0.0 +0,1.816600,0.0,0.0,0.0,0.0,0.0 +0,1.816700,0.0,0.0,0.0,0.0,0.0 +0,1.816800,0.0,0.0,0.0,0.0,0.0 +0,1.816900,0.0,0.0,0.0,0.0,0.0 +0,1.817000,0.0,0.0,0.0,0.0,0.0 +0,1.817100,0.0,0.0,0.0,0.0,0.0 +0,1.817200,0.0,0.0,0.0,0.0,0.0 +0,1.817300,0.0,0.0,0.0,0.0,0.0 +0,1.817400,0.0,0.0,0.0,0.0,0.0 +0,1.817500,0.0,0.0,0.0,0.0,0.0 +0,1.817600,0.0,0.0,0.0,0.0,0.0 +0,1.817700,0.0,0.0,0.0,0.0,0.0 +0,1.817800,0.0,0.0,0.0,0.0,0.0 +0,1.817900,0.0,0.0,0.0,0.0,0.0 +0,1.818000,0.0,0.0,0.0,0.0,0.0 +0,1.818100,0.0,0.0,0.0,0.0,0.0 +0,1.818200,0.0,0.0,0.0,0.0,0.0 +0,1.818300,0.0,0.0,0.0,0.0,0.0 +0,1.818400,0.0,0.0,0.0,0.0,0.0 +0,1.818500,0.0,0.0,0.0,0.0,0.0 +0,1.818600,0.0,0.0,0.0,0.0,0.0 +0,1.818700,0.0,0.0,0.0,0.0,0.0 +0,1.818800,0.0,0.0,0.0,0.0,0.0 +0,1.818900,0.0,0.0,0.0,0.0,0.0 +0,1.819000,0.0,0.0,0.0,0.0,0.0 +0,1.819100,0.0,0.0,0.0,0.0,0.0 +0,1.819200,0.0,0.0,0.0,0.0,0.0 +0,1.819300,0.0,0.0,0.0,0.0,0.0 +0,1.819400,0.0,0.0,0.0,0.0,0.0 +0,1.819500,0.0,0.0,0.0,0.0,0.0 +0,1.819600,0.0,0.0,0.0,0.0,0.0 +0,1.819700,0.0,0.0,0.0,0.0,0.0 +0,1.819800,0.0,0.0,0.0,0.0,0.0 +0,1.819900,0.0,0.0,0.0,0.0,0.0 +0,1.820000,0.0,0.0,0.0,0.0,0.0 +0,1.820100,0.0,0.0,0.0,0.0,0.0 +1,2512.524458,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.820200,0.0,0.0,0.0,0.0,0.0 +0,1.820300,0.0,0.0,0.0,0.0,0.0 +0,1.820400,0.0,0.0,0.0,0.0,0.0 +0,1.820500,0.0,0.0,0.0,0.0,0.0 +0,1.820600,0.0,0.0,0.0,0.0,0.0 +0,1.820700,0.0,0.0,0.0,0.0,0.0 +0,1.820800,0.0,0.0,0.0,0.0,0.0 +0,1.820900,0.0,0.0,0.0,0.0,0.0 +0,1.821000,0.0,0.0,0.0,0.0,0.0 +0,1.821100,0.0,0.0,0.0,0.0,0.0 +0,1.821200,0.0,0.0,0.0,0.0,0.0 +0,1.821300,0.0,0.0,0.0,0.0,0.0 +0,1.821400,0.0,0.0,0.0,0.0,0.0 +0,1.821500,0.0,0.0,0.0,0.0,0.0 +0,1.821600,0.0,0.0,0.0,0.0,0.0 +0,1.821700,0.0,0.0,0.0,0.0,0.0 +0,1.821800,0.0,0.0,0.0,0.0,0.0 +0,1.821900,0.0,0.0,0.0,0.0,0.0 +0,1.822000,0.0,0.0,0.0,0.0,0.0 +0,1.822100,0.0,0.0,0.0,0.0,0.0 +0,1.822200,0.0,0.0,0.0,0.0,0.0 +0,1.822300,0.0,0.0,0.0,0.0,0.0 +0,1.822400,0.0,0.0,0.0,0.0,0.0 +0,1.822500,0.0,0.0,0.0,0.0,0.0 +0,1.822600,0.0,0.0,0.0,0.0,0.0 +0,1.822700,0.0,0.0,0.0,0.0,0.0 +0,1.822800,0.0,0.0,0.0,0.0,0.0 +0,1.822900,0.0,0.0,0.0,0.0,0.0 +0,1.823000,0.0,0.0,0.0,0.0,0.0 +0,1.823100,0.0,0.0,0.0,0.0,0.0 +0,1.823200,0.0,0.0,0.0,0.0,0.0 +0,1.823300,0.0,0.0,0.0,0.0,0.0 +0,1.823400,0.0,0.0,0.0,0.0,0.0 +0,1.823500,0.0,0.0,0.0,0.0,0.0 +0,1.823600,0.0,0.0,0.0,0.0,0.0 +0,1.823700,0.0,0.0,0.0,0.0,0.0 +0,1.823800,0.0,0.0,0.0,0.0,0.0 +0,1.823900,0.0,0.0,0.0,0.0,0.0 +0,1.824000,0.0,0.0,0.0,0.0,0.0 +0,1.824100,0.0,0.0,0.0,0.0,0.0 +0,1.824200,0.0,0.0,0.0,0.0,0.0 +0,1.824300,0.0,0.0,0.0,0.0,0.0 +0,1.824400,0.0,0.0,0.0,0.0,0.0 +0,1.824500,0.0,0.0,0.0,0.0,0.0 +0,1.824600,0.0,0.0,0.0,0.0,0.0 +0,1.824700,0.0,0.0,0.0,0.0,0.0 +0,1.824800,0.0,0.0,0.0,0.0,0.0 +0,1.824900,0.0,0.0,0.0,0.0,0.0 +0,1.825000,0.0,0.0,0.0,0.0,0.0 +0,1.825100,0.0,0.0,0.0,0.0,0.0 +0,1.825200,0.0,0.0,0.0,0.0,0.0 +0,1.825300,0.0,0.0,0.0,0.0,0.0 +0,1.825400,0.0,0.0,0.0,0.0,0.0 +0,1.825500,0.0,0.0,0.0,0.0,0.0 +0,1.825600,0.0,0.0,0.0,0.0,0.0 +0,1.825700,0.0,0.0,0.0,0.0,0.0 +0,1.825800,0.0,0.0,0.0,0.0,0.0 +0,1.825900,0.0,0.0,0.0,0.0,0.0 +0,1.826000,0.0,0.0,0.0,0.0,0.0 +0,1.826100,0.0,0.0,0.0,0.0,0.0 +0,1.826200,0.0,0.0,0.0,0.0,0.0 +0,1.826300,0.0,0.0,0.0,0.0,0.0 +0,1.826400,0.0,0.0,0.0,0.0,0.0 +0,1.826500,0.0,0.0,0.0,0.0,0.0 +0,1.826600,0.0,0.0,0.0,0.0,0.0 +0,1.826700,0.0,0.0,0.0,0.0,0.0 +0,1.826800,0.0,0.0,0.0,0.0,0.0 +0,1.826900,0.0,0.0,0.0,0.0,0.0 +0,1.827000,0.0,0.0,0.0,0.0,0.0 +0,1.827100,0.0,0.0,0.0,0.0,0.0 +0,1.827200,0.0,0.0,0.0,0.0,0.0 +0,1.827300,0.0,0.0,0.0,0.0,0.0 +0,1.827400,0.0,0.0,0.0,0.0,0.0 +0,1.827500,0.0,0.0,0.0,0.0,0.0 +0,1.827600,0.0,0.0,0.0,0.0,0.0 +0,1.827700,0.0,0.0,0.0,0.0,0.0 +0,1.827800,0.0,0.0,0.0,0.0,0.0 +0,1.827900,0.0,0.0,0.0,0.0,0.0 +0,1.828000,0.0,0.0,0.0,0.0,0.0 +0,1.828100,0.0,0.0,0.0,0.0,0.0 +0,1.828200,0.0,0.0,0.0,0.0,0.0 +0,1.828300,0.0,0.0,0.0,0.0,0.0 +0,1.828400,0.0,0.0,0.0,0.0,0.0 +0,1.828500,0.0,0.0,0.0,0.0,0.0 +0,1.828600,0.0,0.0,0.0,0.0,0.0 +0,1.828700,0.0,0.0,0.0,0.0,0.0 +0,1.828800,0.0,0.0,0.0,0.0,0.0 +0,1.828900,0.0,0.0,0.0,0.0,0.0 +0,1.829000,0.0,0.0,0.0,0.0,0.0 +0,1.829100,0.0,0.0,0.0,0.0,0.0 +0,1.829200,0.0,0.0,0.0,0.0,0.0 +0,1.829300,0.0,0.0,0.0,0.0,0.0 +0,1.829400,0.0,0.0,0.0,0.0,0.0 +0,1.829500,0.0,0.0,0.0,0.0,0.0 +0,1.829600,0.0,0.0,0.0,0.0,0.0 +0,1.829700,0.0,0.0,0.0,0.0,0.0 +0,1.829800,0.0,0.0,0.0,0.0,0.0 +0,1.829900,0.0,0.0,0.0,0.0,0.0 +0,1.830000,0.0,0.0,0.0,0.0,0.0 +0,1.830100,0.0,0.0,0.0,0.0,0.0 +0,1.830200,0.0,0.0,0.0,0.0,0.0 +0,1.830300,0.0,0.0,0.0,0.0,0.0 +0,1.830400,0.0,0.0,0.0,0.0,0.0 +0,1.830500,0.0,0.0,0.0,0.0,0.0 +0,1.830600,0.0,0.0,0.0,0.0,0.0 +0,1.830700,0.0,0.0,0.0,0.0,0.0 +0,1.830800,0.0,0.0,0.0,0.0,0.0 +0,1.830900,0.0,0.0,0.0,0.0,0.0 +0,1.831000,0.0,0.0,0.0,0.0,0.0 +0,1.831100,0.0,0.0,0.0,0.0,0.0 +0,1.831200,0.0,0.0,0.0,0.0,0.0 +0,1.831300,0.0,0.0,0.0,0.0,0.0 +0,1.831400,0.0,0.0,0.0,0.0,0.0 +0,1.831500,0.0,0.0,0.0,0.0,0.0 +0,1.831600,0.0,0.0,0.0,0.0,0.0 +0,1.831700,0.0,0.0,0.0,0.0,0.0 +0,1.831800,0.0,0.0,0.0,0.0,0.0 +0,1.831900,0.0,0.0,0.0,0.0,0.0 +0,1.832000,0.0,0.0,0.0,0.0,0.0 +0,1.832100,0.0,0.0,0.0,0.0,0.0 +0,1.832200,0.0,0.0,0.0,0.0,0.0 +0,1.832300,0.0,0.0,0.0,0.0,0.0 +0,1.832400,0.0,0.0,0.0,0.0,0.0 +0,1.832500,0.0,0.0,0.0,0.0,0.0 +0,1.832600,0.0,0.0,0.0,0.0,0.0 +0,1.832700,0.0,0.0,0.0,0.0,0.0 +0,1.832800,0.0,0.0,0.0,0.0,0.0 +0,1.832900,0.0,0.0,0.0,0.0,0.0 +0,1.833000,0.0,0.0,0.0,0.0,0.0 +0,1.833100,0.0,0.0,0.0,0.0,0.0 +0,1.833200,0.0,0.0,0.0,0.0,0.0 +0,1.833300,0.0,0.0,0.0,0.0,0.0 +0,1.833400,0.0,0.0,0.0,0.0,0.0 +0,1.833500,0.0,0.0,0.0,0.0,0.0 +0,1.833600,0.0,0.0,0.0,0.0,0.0 +0,1.833700,0.0,0.0,0.0,0.0,0.0 +0,1.833800,0.0,0.0,0.0,0.0,0.0 +0,1.833900,0.0,0.0,0.0,0.0,0.0 +0,1.834000,0.0,0.0,0.0,0.0,0.0 +0,1.834100,0.0,0.0,0.0,0.0,0.0 +0,1.834200,0.0,0.0,0.0,0.0,0.0 +0,1.834300,0.0,0.0,0.0,0.0,0.0 +0,1.834400,0.0,0.0,0.0,0.0,0.0 +0,1.834500,0.0,0.0,0.0,0.0,0.0 +0,1.834600,0.0,0.0,0.0,0.0,0.0 +0,1.834700,0.0,0.0,0.0,0.0,0.0 +0,1.834800,0.0,0.0,0.0,0.0,0.0 +0,1.834900,0.0,0.0,0.0,0.0,0.0 +0,1.835000,0.0,0.0,0.0,0.0,0.0 +0,1.835100,0.0,0.0,0.0,0.0,0.0 +0,1.835200,0.0,0.0,0.0,0.0,0.0 +0,1.835300,0.0,0.0,0.0,0.0,0.0 +0,1.835400,0.0,0.0,0.0,0.0,0.0 +0,1.835500,0.0,0.0,0.0,0.0,0.0 +0,1.835600,0.0,0.0,0.0,0.0,0.0 +0,1.835700,0.0,0.0,0.0,0.0,0.0 +0,1.835800,0.0,0.0,0.0,0.0,0.0 +0,1.835900,0.0,0.0,0.0,0.0,0.0 +0,1.836000,0.0,0.0,0.0,0.0,0.0 +0,1.836100,0.0,0.0,0.0,0.0,0.0 +0,1.836200,0.0,0.0,0.0,0.0,0.0 +0,1.836300,0.0,0.0,0.0,0.0,0.0 +0,1.836400,0.0,0.0,0.0,0.0,0.0 +0,1.836500,0.0,0.0,0.0,0.0,0.0 +0,1.836600,0.0,0.0,0.0,0.0,0.0 +0,1.836700,0.0,0.0,0.0,0.0,0.0 +0,1.836800,0.0,0.0,0.0,0.0,0.0 +0,1.836900,0.0,0.0,0.0,0.0,0.0 +0,1.837000,0.0,0.0,0.0,0.0,0.0 +0,1.837100,0.0,0.0,0.0,0.0,0.0 +0,1.837200,0.0,0.0,0.0,0.0,0.0 +0,1.837300,0.0,0.0,0.0,0.0,0.0 +0,1.837400,0.0,0.0,0.0,0.0,0.0 +0,1.837500,0.0,0.0,0.0,0.0,0.0 +0,1.837600,0.0,0.0,0.0,0.0,0.0 +0,1.837700,0.0,0.0,0.0,0.0,0.0 +0,1.837800,0.0,0.0,0.0,0.0,0.0 +0,1.837900,0.0,0.0,0.0,0.0,0.0 +0,1.838000,0.0,0.0,0.0,0.0,0.0 +0,1.838100,0.0,0.0,0.0,0.0,0.0 +0,1.838200,0.0,0.0,0.0,0.0,0.0 +0,1.838300,0.0,0.0,0.0,0.0,0.0 +0,1.838400,0.0,0.0,0.0,0.0,0.0 +0,1.838500,0.0,0.0,0.0,0.0,0.0 +0,1.838600,0.0,0.0,0.0,0.0,0.0 +0,1.838700,0.0,0.0,0.0,0.0,0.0 +0,1.838800,0.0,0.0,0.0,0.0,0.0 +0,1.838900,0.0,0.0,0.0,0.0,0.0 +0,1.839000,0.0,0.0,0.0,0.0,0.0 +0,1.839100,0.0,0.0,0.0,0.0,0.0 +0,1.839200,0.0,0.0,0.0,0.0,0.0 +0,1.839300,0.0,0.0,0.0,0.0,0.0 +0,1.839400,0.0,0.0,0.0,0.0,0.0 +0,1.839500,0.0,0.0,0.0,0.0,0.0 +0,1.839600,0.0,0.0,0.0,0.0,0.0 +0,1.839700,0.0,0.0,0.0,0.0,0.0 +0,1.839800,0.0,0.0,0.0,0.0,0.0 +0,1.839900,0.0,0.0,0.0,0.0,0.0 +0,1.840000,0.0,0.0,0.0,0.0,0.0 +0,1.840100,0.0,0.0,0.0,0.0,0.0 +1,2596.261517,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.840200,0.0,0.0,0.0,0.0,0.0 +0,1.840300,0.0,0.0,0.0,0.0,0.0 +0,1.840400,0.0,0.0,0.0,0.0,0.0 +0,1.840500,0.0,0.0,0.0,0.0,0.0 +0,1.840600,0.0,0.0,0.0,0.0,0.0 +0,1.840700,0.0,0.0,0.0,0.0,0.0 +0,1.840800,0.0,0.0,0.0,0.0,0.0 +0,1.840900,0.0,0.0,0.0,0.0,0.0 +0,1.841000,0.0,0.0,0.0,0.0,0.0 +0,1.841100,0.0,0.0,0.0,0.0,0.0 +0,1.841200,0.0,0.0,0.0,0.0,0.0 +0,1.841300,0.0,0.0,0.0,0.0,0.0 +0,1.841400,0.0,0.0,0.0,0.0,0.0 +0,1.841500,0.0,0.0,0.0,0.0,0.0 +0,1.841600,0.0,0.0,0.0,0.0,0.0 +0,1.841700,0.0,0.0,0.0,0.0,0.0 +0,1.841800,0.0,0.0,0.0,0.0,0.0 +0,1.841900,0.0,0.0,0.0,0.0,0.0 +0,1.842000,0.0,0.0,0.0,0.0,0.0 +0,1.842100,0.0,0.0,0.0,0.0,0.0 +0,1.842200,0.0,0.0,0.0,0.0,0.0 +0,1.842300,0.0,0.0,0.0,0.0,0.0 +0,1.842400,0.0,0.0,0.0,0.0,0.0 +0,1.842500,0.0,0.0,0.0,0.0,0.0 +0,1.842600,0.0,0.0,0.0,0.0,0.0 +0,1.842700,0.0,0.0,0.0,0.0,0.0 +0,1.842800,0.0,0.0,0.0,0.0,0.0 +0,1.842900,0.0,0.0,0.0,0.0,0.0 +0,1.843000,0.0,0.0,0.0,0.0,0.0 +0,1.843100,0.0,0.0,0.0,0.0,0.0 +0,1.843200,0.0,0.0,0.0,0.0,0.0 +0,1.843300,0.0,0.0,0.0,0.0,0.0 +0,1.843400,0.0,0.0,0.0,0.0,0.0 +0,1.843500,0.0,0.0,0.0,0.0,0.0 +0,1.843600,0.0,0.0,0.0,0.0,0.0 +0,1.843700,0.0,0.0,0.0,0.0,0.0 +0,1.843800,0.0,0.0,0.0,0.0,0.0 +0,1.843900,0.0,0.0,0.0,0.0,0.0 +0,1.844000,0.0,0.0,0.0,0.0,0.0 +0,1.844100,0.0,0.0,0.0,0.0,0.0 +0,1.844200,0.0,0.0,0.0,0.0,0.0 +0,1.844300,0.0,0.0,0.0,0.0,0.0 +0,1.844400,0.0,0.0,0.0,0.0,0.0 +0,1.844500,0.0,0.0,0.0,0.0,0.0 +0,1.844600,0.0,0.0,0.0,0.0,0.0 +0,1.844700,0.0,0.0,0.0,0.0,0.0 +0,1.844800,0.0,0.0,0.0,0.0,0.0 +0,1.844900,0.0,0.0,0.0,0.0,0.0 +0,1.845000,0.0,0.0,0.0,0.0,0.0 +0,1.845100,0.0,0.0,0.0,0.0,0.0 +0,1.845200,0.0,0.0,0.0,0.0,0.0 +0,1.845300,0.0,0.0,0.0,0.0,0.0 +0,1.845400,0.0,0.0,0.0,0.0,0.0 +0,1.845500,0.0,0.0,0.0,0.0,0.0 +0,1.845600,0.0,0.0,0.0,0.0,0.0 +0,1.845700,0.0,0.0,0.0,0.0,0.0 +0,1.845800,0.0,0.0,0.0,0.0,0.0 +0,1.845900,0.0,0.0,0.0,0.0,0.0 +0,1.846000,0.0,0.0,0.0,0.0,0.0 +0,1.846100,0.0,0.0,0.0,0.0,0.0 +0,1.846200,0.0,0.0,0.0,0.0,0.0 +0,1.846300,0.0,0.0,0.0,0.0,0.0 +0,1.846400,0.0,0.0,0.0,0.0,0.0 +0,1.846500,0.0,0.0,0.0,0.0,0.0 +0,1.846600,0.0,0.0,0.0,0.0,0.0 +0,1.846700,0.0,0.0,0.0,0.0,0.0 +0,1.846800,0.0,0.0,0.0,0.0,0.0 +0,1.846900,0.0,0.0,0.0,0.0,0.0 +0,1.847000,0.0,0.0,0.0,0.0,0.0 +0,1.847100,0.0,0.0,0.0,0.0,0.0 +0,1.847200,0.0,0.0,0.0,0.0,0.0 +0,1.847300,0.0,0.0,0.0,0.0,0.0 +0,1.847400,0.0,0.0,0.0,0.0,0.0 +0,1.847500,0.0,0.0,0.0,0.0,0.0 +0,1.847600,0.0,0.0,0.0,0.0,0.0 +0,1.847700,0.0,0.0,0.0,0.0,0.0 +0,1.847800,0.0,0.0,0.0,0.0,0.0 +0,1.847900,0.0,0.0,0.0,0.0,0.0 +0,1.848000,0.0,0.0,0.0,0.0,0.0 +0,1.848100,0.0,0.0,0.0,0.0,0.0 +0,1.848200,0.0,0.0,0.0,0.0,0.0 +0,1.848300,0.0,0.0,0.0,0.0,0.0 +0,1.848400,0.0,0.0,0.0,0.0,0.0 +0,1.848500,0.0,0.0,0.0,0.0,0.0 +0,1.848600,0.0,0.0,0.0,0.0,0.0 +0,1.848700,0.0,0.0,0.0,0.0,0.0 +0,1.848800,0.0,0.0,0.0,0.0,0.0 +0,1.848900,0.0,0.0,0.0,0.0,0.0 +0,1.849000,0.0,0.0,0.0,0.0,0.0 +0,1.849100,0.0,0.0,0.0,0.0,0.0 +0,1.849200,0.0,0.0,0.0,0.0,0.0 +0,1.849300,0.0,0.0,0.0,0.0,0.0 +0,1.849400,0.0,0.0,0.0,0.0,0.0 +0,1.849500,0.0,0.0,0.0,0.0,0.0 +0,1.849600,0.0,0.0,0.0,0.0,0.0 +0,1.849700,0.0,0.0,0.0,0.0,0.0 +0,1.849800,0.0,0.0,0.0,0.0,0.0 +0,1.849900,0.0,0.0,0.0,0.0,0.0 +0,1.850000,0.0,0.0,0.0,0.0,0.0 +0,1.850100,0.0,0.0,0.0,0.0,0.0 +0,1.850200,0.0,0.0,0.0,0.0,0.0 +0,1.850300,0.0,0.0,0.0,0.0,0.0 +0,1.850400,0.0,0.0,0.0,0.0,0.0 +0,1.850500,0.0,0.0,0.0,0.0,0.0 +0,1.850600,0.0,0.0,0.0,0.0,0.0 +0,1.850700,0.0,0.0,0.0,0.0,0.0 +0,1.850800,0.0,0.0,0.0,0.0,0.0 +0,1.850900,0.0,0.0,0.0,0.0,0.0 +0,1.851000,0.0,0.0,0.0,0.0,0.0 +0,1.851100,0.0,0.0,0.0,0.0,0.0 +0,1.851200,0.0,0.0,0.0,0.0,0.0 +0,1.851300,0.0,0.0,0.0,0.0,0.0 +0,1.851400,0.0,0.0,0.0,0.0,0.0 +0,1.851500,0.0,0.0,0.0,0.0,0.0 +0,1.851600,0.0,0.0,0.0,0.0,0.0 +0,1.851700,0.0,0.0,0.0,0.0,0.0 +0,1.851800,0.0,0.0,0.0,0.0,0.0 +0,1.851900,0.0,0.0,0.0,0.0,0.0 +0,1.852000,0.0,0.0,0.0,0.0,0.0 +0,1.852100,0.0,0.0,0.0,0.0,0.0 +0,1.852200,0.0,0.0,0.0,0.0,0.0 +0,1.852300,0.0,0.0,0.0,0.0,0.0 +0,1.852400,0.0,0.0,0.0,0.0,0.0 +0,1.852500,0.0,0.0,0.0,0.0,0.0 +0,1.852600,0.0,0.0,0.0,0.0,0.0 +0,1.852700,0.0,0.0,0.0,0.0,0.0 +0,1.852800,0.0,0.0,0.0,0.0,0.0 +0,1.852900,0.0,0.0,0.0,0.0,0.0 +0,1.853000,0.0,0.0,0.0,0.0,0.0 +0,1.853100,0.0,0.0,0.0,0.0,0.0 +0,1.853200,0.0,0.0,0.0,0.0,0.0 +0,1.853300,0.0,0.0,0.0,0.0,0.0 +0,1.853400,0.0,0.0,0.0,0.0,0.0 +0,1.853500,0.0,0.0,0.0,0.0,0.0 +0,1.853600,0.0,0.0,0.0,0.0,0.0 +0,1.853700,0.0,0.0,0.0,0.0,0.0 +0,1.853800,0.0,0.0,0.0,0.0,0.0 +0,1.853900,0.0,0.0,0.0,0.0,0.0 +0,1.854000,0.0,0.0,0.0,0.0,0.0 +0,1.854100,0.0,0.0,0.0,0.0,0.0 +0,1.854200,0.0,0.0,0.0,0.0,0.0 +0,1.854300,0.0,0.0,0.0,0.0,0.0 +0,1.854400,0.0,0.0,0.0,0.0,0.0 +0,1.854500,0.0,0.0,0.0,0.0,0.0 +0,1.854600,0.0,0.0,0.0,0.0,0.0 +0,1.854700,0.0,0.0,0.0,0.0,0.0 +0,1.854800,0.0,0.0,0.0,0.0,0.0 +0,1.854900,0.0,0.0,0.0,0.0,0.0 +0,1.855000,0.0,0.0,0.0,0.0,0.0 +0,1.855100,0.0,0.0,0.0,0.0,0.0 +0,1.855200,0.0,0.0,0.0,0.0,0.0 +0,1.855300,0.0,0.0,0.0,0.0,0.0 +0,1.855400,0.0,0.0,0.0,0.0,0.0 +0,1.855500,0.0,0.0,0.0,0.0,0.0 +0,1.855600,0.0,0.0,0.0,0.0,0.0 +0,1.855700,0.0,0.0,0.0,0.0,0.0 +0,1.855800,0.0,0.0,0.0,0.0,0.0 +0,1.855900,0.0,0.0,0.0,0.0,0.0 +0,1.856000,0.0,0.0,0.0,0.0,0.0 +0,1.856100,0.0,0.0,0.0,0.0,0.0 +0,1.856200,0.0,0.0,0.0,0.0,0.0 +0,1.856300,0.0,0.0,0.0,0.0,0.0 +0,1.856400,0.0,0.0,0.0,0.0,0.0 +0,1.856500,0.0,0.0,0.0,0.0,0.0 +0,1.856600,0.0,0.0,0.0,0.0,0.0 +0,1.856700,0.0,0.0,0.0,0.0,0.0 +0,1.856800,0.0,0.0,0.0,0.0,0.0 +0,1.856900,0.0,0.0,0.0,0.0,0.0 +0,1.857000,0.0,0.0,0.0,0.0,0.0 +0,1.857100,0.0,0.0,0.0,0.0,0.0 +0,1.857200,0.0,0.0,0.0,0.0,0.0 +0,1.857300,0.0,0.0,0.0,0.0,0.0 +0,1.857400,0.0,0.0,0.0,0.0,0.0 +0,1.857500,0.0,0.0,0.0,0.0,0.0 +0,1.857600,0.0,0.0,0.0,0.0,0.0 +0,1.857700,0.0,0.0,0.0,0.0,0.0 +0,1.857800,0.0,0.0,0.0,0.0,0.0 +0,1.857900,0.0,0.0,0.0,0.0,0.0 +0,1.858000,0.0,0.0,0.0,0.0,0.0 +0,1.858100,0.0,0.0,0.0,0.0,0.0 +0,1.858200,0.0,0.0,0.0,0.0,0.0 +0,1.858300,0.0,0.0,0.0,0.0,0.0 +0,1.858400,0.0,0.0,0.0,0.0,0.0 +0,1.858500,0.0,0.0,0.0,0.0,0.0 +0,1.858600,0.0,0.0,0.0,0.0,0.0 +0,1.858700,0.0,0.0,0.0,0.0,0.0 +0,1.858800,0.0,0.0,0.0,0.0,0.0 +0,1.858900,0.0,0.0,0.0,0.0,0.0 +0,1.859000,0.0,0.0,0.0,0.0,0.0 +0,1.859100,0.0,0.0,0.0,0.0,0.0 +0,1.859200,0.0,0.0,0.0,0.0,0.0 +0,1.859300,0.0,0.0,0.0,0.0,0.0 +0,1.859400,0.0,0.0,0.0,0.0,0.0 +0,1.859500,0.0,0.0,0.0,0.0,0.0 +0,1.859600,0.0,0.0,0.0,0.0,0.0 +0,1.859700,0.0,0.0,0.0,0.0,0.0 +0,1.859800,0.0,0.0,0.0,0.0,0.0 +0,1.859900,0.0,0.0,0.0,0.0,0.0 +0,1.860000,0.0,0.0,0.0,0.0,0.0 +0,1.860100,0.0,0.0,0.0,0.0,0.0 +1,2681.838725,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.860200,0.0,0.0,0.0,0.0,0.0 +0,1.860300,0.0,0.0,0.0,0.0,0.0 +0,1.860400,0.0,0.0,0.0,0.0,0.0 +0,1.860500,0.0,0.0,0.0,0.0,0.0 +0,1.860600,0.0,0.0,0.0,0.0,0.0 +0,1.860700,0.0,0.0,0.0,0.0,0.0 +0,1.860800,0.0,0.0,0.0,0.0,0.0 +0,1.860900,0.0,0.0,0.0,0.0,0.0 +0,1.861000,0.0,0.0,0.0,0.0,0.0 +0,1.861100,0.0,0.0,0.0,0.0,0.0 +0,1.861200,0.0,0.0,0.0,0.0,0.0 +0,1.861300,0.0,0.0,0.0,0.0,0.0 +0,1.861400,0.0,0.0,0.0,0.0,0.0 +0,1.861500,0.0,0.0,0.0,0.0,0.0 +0,1.861600,0.0,0.0,0.0,0.0,0.0 +0,1.861700,0.0,0.0,0.0,0.0,0.0 +0,1.861800,0.0,0.0,0.0,0.0,0.0 +0,1.861900,0.0,0.0,0.0,0.0,0.0 +0,1.862000,0.0,0.0,0.0,0.0,0.0 +0,1.862100,0.0,0.0,0.0,0.0,0.0 +0,1.862200,0.0,0.0,0.0,0.0,0.0 +0,1.862300,0.0,0.0,0.0,0.0,0.0 +0,1.862400,0.0,0.0,0.0,0.0,0.0 +0,1.862500,0.0,0.0,0.0,0.0,0.0 +0,1.862600,0.0,0.0,0.0,0.0,0.0 +0,1.862700,0.0,0.0,0.0,0.0,0.0 +0,1.862800,0.0,0.0,0.0,0.0,0.0 +0,1.862900,0.0,0.0,0.0,0.0,0.0 +0,1.863000,0.0,0.0,0.0,0.0,0.0 +0,1.863100,0.0,0.0,0.0,0.0,0.0 +0,1.863200,0.0,0.0,0.0,0.0,0.0 +0,1.863300,0.0,0.0,0.0,0.0,0.0 +0,1.863400,0.0,0.0,0.0,0.0,0.0 +0,1.863500,0.0,0.0,0.0,0.0,0.0 +0,1.863600,0.0,0.0,0.0,0.0,0.0 +0,1.863700,0.0,0.0,0.0,0.0,0.0 +0,1.863800,0.0,0.0,0.0,0.0,0.0 +0,1.863900,0.0,0.0,0.0,0.0,0.0 +0,1.864000,0.0,0.0,0.0,0.0,0.0 +0,1.864100,0.0,0.0,0.0,0.0,0.0 +0,1.864200,0.0,0.0,0.0,0.0,0.0 +0,1.864300,0.0,0.0,0.0,0.0,0.0 +0,1.864400,0.0,0.0,0.0,0.0,0.0 +0,1.864500,0.0,0.0,0.0,0.0,0.0 +0,1.864600,0.0,0.0,0.0,0.0,0.0 +0,1.864700,0.0,0.0,0.0,0.0,0.0 +0,1.864800,0.0,0.0,0.0,0.0,0.0 +0,1.864900,0.0,0.0,0.0,0.0,0.0 +0,1.865000,0.0,0.0,0.0,0.0,0.0 +0,1.865100,0.0,0.0,0.0,0.0,0.0 +0,1.865200,0.0,0.0,0.0,0.0,0.0 +0,1.865300,0.0,0.0,0.0,0.0,0.0 +0,1.865400,0.0,0.0,0.0,0.0,0.0 +0,1.865500,0.0,0.0,0.0,0.0,0.0 +0,1.865600,0.0,0.0,0.0,0.0,0.0 +0,1.865700,0.0,0.0,0.0,0.0,0.0 +0,1.865800,0.0,0.0,0.0,0.0,0.0 +0,1.865900,0.0,0.0,0.0,0.0,0.0 +0,1.866000,0.0,0.0,0.0,0.0,0.0 +0,1.866100,0.0,0.0,0.0,0.0,0.0 +0,1.866200,0.0,0.0,0.0,0.0,0.0 +0,1.866300,0.0,0.0,0.0,0.0,0.0 +0,1.866400,0.0,0.0,0.0,0.0,0.0 +0,1.866500,0.0,0.0,0.0,0.0,0.0 +0,1.866600,0.0,0.0,0.0,0.0,0.0 +0,1.866700,0.0,0.0,0.0,0.0,0.0 +0,1.866800,0.0,0.0,0.0,0.0,0.0 +0,1.866900,0.0,0.0,0.0,0.0,0.0 +0,1.867000,0.0,0.0,0.0,0.0,0.0 +0,1.867100,0.0,0.0,0.0,0.0,0.0 +0,1.867200,0.0,0.0,0.0,0.0,0.0 +0,1.867300,0.0,0.0,0.0,0.0,0.0 +0,1.867400,0.0,0.0,0.0,0.0,0.0 +0,1.867500,0.0,0.0,0.0,0.0,0.0 +0,1.867600,0.0,0.0,0.0,0.0,0.0 +0,1.867700,0.0,0.0,0.0,0.0,0.0 +0,1.867800,0.0,0.0,0.0,0.0,0.0 +0,1.867900,0.0,0.0,0.0,0.0,0.0 +0,1.868000,0.0,0.0,0.0,0.0,0.0 +0,1.868100,0.0,0.0,0.0,0.0,0.0 +0,1.868200,0.0,0.0,0.0,0.0,0.0 +0,1.868300,0.0,0.0,0.0,0.0,0.0 +0,1.868400,0.0,0.0,0.0,0.0,0.0 +0,1.868500,0.0,0.0,0.0,0.0,0.0 +0,1.868600,0.0,0.0,0.0,0.0,0.0 +0,1.868700,0.0,0.0,0.0,0.0,0.0 +0,1.868800,0.0,0.0,0.0,0.0,0.0 +0,1.868900,0.0,0.0,0.0,0.0,0.0 +0,1.869000,0.0,0.0,0.0,0.0,0.0 +0,1.869100,0.0,0.0,0.0,0.0,0.0 +0,1.869200,0.0,0.0,0.0,0.0,0.0 +0,1.869300,0.0,0.0,0.0,0.0,0.0 +0,1.869400,0.0,0.0,0.0,0.0,0.0 +0,1.869500,0.0,0.0,0.0,0.0,0.0 +0,1.869600,0.0,0.0,0.0,0.0,0.0 +0,1.869700,0.0,0.0,0.0,0.0,0.0 +0,1.869800,0.0,0.0,0.0,0.0,0.0 +0,1.869900,0.0,0.0,0.0,0.0,0.0 +0,1.870000,0.0,0.0,0.0,0.0,0.0 +0,1.870100,0.0,0.0,0.0,0.0,0.0 +0,1.870200,0.0,0.0,0.0,0.0,0.0 +0,1.870300,0.0,0.0,0.0,0.0,0.0 +0,1.870400,0.0,0.0,0.0,0.0,0.0 +0,1.870500,0.0,0.0,0.0,0.0,0.0 +0,1.870600,0.0,0.0,0.0,0.0,0.0 +0,1.870700,0.0,0.0,0.0,0.0,0.0 +0,1.870800,0.0,0.0,0.0,0.0,0.0 +0,1.870900,0.0,0.0,0.0,0.0,0.0 +0,1.871000,0.0,0.0,0.0,0.0,0.0 +0,1.871100,0.0,0.0,0.0,0.0,0.0 +0,1.871200,0.0,0.0,0.0,0.0,0.0 +0,1.871300,0.0,0.0,0.0,0.0,0.0 +0,1.871400,0.0,0.0,0.0,0.0,0.0 +0,1.871500,0.0,0.0,0.0,0.0,0.0 +0,1.871600,0.0,0.0,0.0,0.0,0.0 +0,1.871700,0.0,0.0,0.0,0.0,0.0 +0,1.871800,0.0,0.0,0.0,0.0,0.0 +0,1.871900,0.0,0.0,0.0,0.0,0.0 +0,1.872000,0.0,0.0,0.0,0.0,0.0 +0,1.872100,0.0,0.0,0.0,0.0,0.0 +0,1.872200,0.0,0.0,0.0,0.0,0.0 +0,1.872300,0.0,0.0,0.0,0.0,0.0 +0,1.872400,0.0,0.0,0.0,0.0,0.0 +0,1.872500,0.0,0.0,0.0,0.0,0.0 +0,1.872600,0.0,0.0,0.0,0.0,0.0 +0,1.872700,0.0,0.0,0.0,0.0,0.0 +0,1.872800,0.0,0.0,0.0,0.0,0.0 +0,1.872900,0.0,0.0,0.0,0.0,0.0 +0,1.873000,0.0,0.0,0.0,0.0,0.0 +0,1.873100,0.0,0.0,0.0,0.0,0.0 +0,1.873200,0.0,0.0,0.0,0.0,0.0 +0,1.873300,0.0,0.0,0.0,0.0,0.0 +0,1.873400,0.0,0.0,0.0,0.0,0.0 +0,1.873500,0.0,0.0,0.0,0.0,0.0 +0,1.873600,0.0,0.0,0.0,0.0,0.0 +0,1.873700,0.0,0.0,0.0,0.0,0.0 +0,1.873800,0.0,0.0,0.0,0.0,0.0 +0,1.873900,0.0,0.0,0.0,0.0,0.0 +0,1.874000,0.0,0.0,0.0,0.0,0.0 +0,1.874100,0.0,0.0,0.0,0.0,0.0 +0,1.874200,0.0,0.0,0.0,0.0,0.0 +0,1.874300,0.0,0.0,0.0,0.0,0.0 +0,1.874400,0.0,0.0,0.0,0.0,0.0 +0,1.874500,0.0,0.0,0.0,0.0,0.0 +0,1.874600,0.0,0.0,0.0,0.0,0.0 +0,1.874700,0.0,0.0,0.0,0.0,0.0 +0,1.874800,0.0,0.0,0.0,0.0,0.0 +0,1.874900,0.0,0.0,0.0,0.0,0.0 +0,1.875000,0.0,0.0,0.0,0.0,0.0 +0,1.875100,0.0,0.0,0.0,0.0,0.0 +0,1.875200,0.0,0.0,0.0,0.0,0.0 +0,1.875300,0.0,0.0,0.0,0.0,0.0 +0,1.875400,0.0,0.0,0.0,0.0,0.0 +0,1.875500,0.0,0.0,0.0,0.0,0.0 +0,1.875600,0.0,0.0,0.0,0.0,0.0 +0,1.875700,0.0,0.0,0.0,0.0,0.0 +0,1.875800,0.0,0.0,0.0,0.0,0.0 +0,1.875900,0.0,0.0,0.0,0.0,0.0 +0,1.876000,0.0,0.0,0.0,0.0,0.0 +0,1.876100,0.0,0.0,0.0,0.0,0.0 +0,1.876200,0.0,0.0,0.0,0.0,0.0 +0,1.876300,0.0,0.0,0.0,0.0,0.0 +0,1.876400,0.0,0.0,0.0,0.0,0.0 +0,1.876500,0.0,0.0,0.0,0.0,0.0 +0,1.876600,0.0,0.0,0.0,0.0,0.0 +0,1.876700,0.0,0.0,0.0,0.0,0.0 +0,1.876800,0.0,0.0,0.0,0.0,0.0 +0,1.876900,0.0,0.0,0.0,0.0,0.0 +0,1.877000,0.0,0.0,0.0,0.0,0.0 +0,1.877100,0.0,0.0,0.0,0.0,0.0 +0,1.877200,0.0,0.0,0.0,0.0,0.0 +0,1.877300,0.0,0.0,0.0,0.0,0.0 +0,1.877400,0.0,0.0,0.0,0.0,0.0 +0,1.877500,0.0,0.0,0.0,0.0,0.0 +0,1.877600,0.0,0.0,0.0,0.0,0.0 +0,1.877700,0.0,0.0,0.0,0.0,0.0 +0,1.877800,0.0,0.0,0.0,0.0,0.0 +0,1.877900,0.0,0.0,0.0,0.0,0.0 +0,1.878000,0.0,0.0,0.0,0.0,0.0 +0,1.878100,0.0,0.0,0.0,0.0,0.0 +0,1.878200,0.0,0.0,0.0,0.0,0.0 +0,1.878300,0.0,0.0,0.0,0.0,0.0 +0,1.878400,0.0,0.0,0.0,0.0,0.0 +0,1.878500,0.0,0.0,0.0,0.0,0.0 +0,1.878600,0.0,0.0,0.0,0.0,0.0 +0,1.878700,0.0,0.0,0.0,0.0,0.0 +0,1.878800,0.0,0.0,0.0,0.0,0.0 +0,1.878900,0.0,0.0,0.0,0.0,0.0 +0,1.879000,0.0,0.0,0.0,0.0,0.0 +0,1.879100,0.0,0.0,0.0,0.0,0.0 +0,1.879200,0.0,0.0,0.0,0.0,0.0 +0,1.879300,0.0,0.0,0.0,0.0,0.0 +0,1.879400,0.0,0.0,0.0,0.0,0.0 +0,1.879500,0.0,0.0,0.0,0.0,0.0 +0,1.879600,0.0,0.0,0.0,0.0,0.0 +0,1.879700,0.0,0.0,0.0,0.0,0.0 +0,1.879800,0.0,0.0,0.0,0.0,0.0 +0,1.879900,0.0,0.0,0.0,0.0,0.0 +0,1.880000,0.0,0.0,0.0,0.0,0.0 +0,1.880100,0.0,0.0,0.0,0.0,0.0 +1,2769.276084,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.880200,0.0,0.0,0.0,0.0,0.0 +0,1.880300,0.0,0.0,0.0,0.0,0.0 +0,1.880400,0.0,0.0,0.0,0.0,0.0 +0,1.880500,0.0,0.0,0.0,0.0,0.0 +0,1.880600,0.0,0.0,0.0,0.0,0.0 +0,1.880700,0.0,0.0,0.0,0.0,0.0 +0,1.880800,0.0,0.0,0.0,0.0,0.0 +0,1.880900,0.0,0.0,0.0,0.0,0.0 +0,1.881000,0.0,0.0,0.0,0.0,0.0 +0,1.881100,0.0,0.0,0.0,0.0,0.0 +0,1.881200,0.0,0.0,0.0,0.0,0.0 +0,1.881300,0.0,0.0,0.0,0.0,0.0 +0,1.881400,0.0,0.0,0.0,0.0,0.0 +0,1.881500,0.0,0.0,0.0,0.0,0.0 +0,1.881600,0.0,0.0,0.0,0.0,0.0 +0,1.881700,0.0,0.0,0.0,0.0,0.0 +0,1.881800,0.0,0.0,0.0,0.0,0.0 +0,1.881900,0.0,0.0,0.0,0.0,0.0 +0,1.882000,0.0,0.0,0.0,0.0,0.0 +0,1.882100,0.0,0.0,0.0,0.0,0.0 +0,1.882200,0.0,0.0,0.0,0.0,0.0 +0,1.882300,0.0,0.0,0.0,0.0,0.0 +0,1.882400,0.0,0.0,0.0,0.0,0.0 +0,1.882500,0.0,0.0,0.0,0.0,0.0 +0,1.882600,0.0,0.0,0.0,0.0,0.0 +0,1.882700,0.0,0.0,0.0,0.0,0.0 +0,1.882800,0.0,0.0,0.0,0.0,0.0 +0,1.882900,0.0,0.0,0.0,0.0,0.0 +0,1.883000,0.0,0.0,0.0,0.0,0.0 +0,1.883100,0.0,0.0,0.0,0.0,0.0 +0,1.883200,0.0,0.0,0.0,0.0,0.0 +0,1.883300,0.0,0.0,0.0,0.0,0.0 +0,1.883400,0.0,0.0,0.0,0.0,0.0 +0,1.883500,0.0,0.0,0.0,0.0,0.0 +0,1.883600,0.0,0.0,0.0,0.0,0.0 +0,1.883700,0.0,0.0,0.0,0.0,0.0 +0,1.883800,0.0,0.0,0.0,0.0,0.0 +0,1.883900,0.0,0.0,0.0,0.0,0.0 +0,1.884000,0.0,0.0,0.0,0.0,0.0 +0,1.884100,0.0,0.0,0.0,0.0,0.0 +0,1.884200,0.0,0.0,0.0,0.0,0.0 +0,1.884300,0.0,0.0,0.0,0.0,0.0 +0,1.884400,0.0,0.0,0.0,0.0,0.0 +0,1.884500,0.0,0.0,0.0,0.0,0.0 +0,1.884600,0.0,0.0,0.0,0.0,0.0 +0,1.884700,0.0,0.0,0.0,0.0,0.0 +0,1.884800,0.0,0.0,0.0,0.0,0.0 +0,1.884900,0.0,0.0,0.0,0.0,0.0 +0,1.885000,0.0,0.0,0.0,0.0,0.0 +0,1.885100,0.0,0.0,0.0,0.0,0.0 +0,1.885200,0.0,0.0,0.0,0.0,0.0 +0,1.885300,0.0,0.0,0.0,0.0,0.0 +0,1.885400,0.0,0.0,0.0,0.0,0.0 +0,1.885500,0.0,0.0,0.0,0.0,0.0 +0,1.885600,0.0,0.0,0.0,0.0,0.0 +0,1.885700,0.0,0.0,0.0,0.0,0.0 +0,1.885800,0.0,0.0,0.0,0.0,0.0 +0,1.885900,0.0,0.0,0.0,0.0,0.0 +0,1.886000,0.0,0.0,0.0,0.0,0.0 +0,1.886100,0.0,0.0,0.0,0.0,0.0 +0,1.886200,0.0,0.0,0.0,0.0,0.0 +0,1.886300,0.0,0.0,0.0,0.0,0.0 +0,1.886400,0.0,0.0,0.0,0.0,0.0 +0,1.886500,0.0,0.0,0.0,0.0,0.0 +0,1.886600,0.0,0.0,0.0,0.0,0.0 +0,1.886700,0.0,0.0,0.0,0.0,0.0 +0,1.886800,0.0,0.0,0.0,0.0,0.0 +0,1.886900,0.0,0.0,0.0,0.0,0.0 +0,1.887000,0.0,0.0,0.0,0.0,0.0 +0,1.887100,0.0,0.0,0.0,0.0,0.0 +0,1.887200,0.0,0.0,0.0,0.0,0.0 +0,1.887300,0.0,0.0,0.0,0.0,0.0 +0,1.887400,0.0,0.0,0.0,0.0,0.0 +0,1.887500,0.0,0.0,0.0,0.0,0.0 +0,1.887600,0.0,0.0,0.0,0.0,0.0 +0,1.887700,0.0,0.0,0.0,0.0,0.0 +0,1.887800,0.0,0.0,0.0,0.0,0.0 +0,1.887900,0.0,0.0,0.0,0.0,0.0 +0,1.888000,0.0,0.0,0.0,0.0,0.0 +0,1.888100,0.0,0.0,0.0,0.0,0.0 +0,1.888200,0.0,0.0,0.0,0.0,0.0 +0,1.888300,0.0,0.0,0.0,0.0,0.0 +0,1.888400,0.0,0.0,0.0,0.0,0.0 +0,1.888500,0.0,0.0,0.0,0.0,0.0 +0,1.888600,0.0,0.0,0.0,0.0,0.0 +0,1.888700,0.0,0.0,0.0,0.0,0.0 +0,1.888800,0.0,0.0,0.0,0.0,0.0 +0,1.888900,0.0,0.0,0.0,0.0,0.0 +0,1.889000,0.0,0.0,0.0,0.0,0.0 +0,1.889100,0.0,0.0,0.0,0.0,0.0 +0,1.889200,0.0,0.0,0.0,0.0,0.0 +0,1.889300,0.0,0.0,0.0,0.0,0.0 +0,1.889400,0.0,0.0,0.0,0.0,0.0 +0,1.889500,0.0,0.0,0.0,0.0,0.0 +0,1.889600,0.0,0.0,0.0,0.0,0.0 +0,1.889700,0.0,0.0,0.0,0.0,0.0 +0,1.889800,0.0,0.0,0.0,0.0,0.0 +0,1.889900,0.0,0.0,0.0,0.0,0.0 +0,1.890000,0.0,0.0,0.0,0.0,0.0 +0,1.890100,0.0,0.0,0.0,0.0,0.0 +0,1.890200,0.0,0.0,0.0,0.0,0.0 +0,1.890300,0.0,0.0,0.0,0.0,0.0 +0,1.890400,0.0,0.0,0.0,0.0,0.0 +0,1.890500,0.0,0.0,0.0,0.0,0.0 +0,1.890600,0.0,0.0,0.0,0.0,0.0 +0,1.890700,0.0,0.0,0.0,0.0,0.0 +0,1.890800,0.0,0.0,0.0,0.0,0.0 +0,1.890900,0.0,0.0,0.0,0.0,0.0 +0,1.891000,0.0,0.0,0.0,0.0,0.0 +0,1.891100,0.0,0.0,0.0,0.0,0.0 +0,1.891200,0.0,0.0,0.0,0.0,0.0 +0,1.891300,0.0,0.0,0.0,0.0,0.0 +0,1.891400,0.0,0.0,0.0,0.0,0.0 +0,1.891500,0.0,0.0,0.0,0.0,0.0 +0,1.891600,0.0,0.0,0.0,0.0,0.0 +0,1.891700,0.0,0.0,0.0,0.0,0.0 +0,1.891800,0.0,0.0,0.0,0.0,0.0 +0,1.891900,0.0,0.0,0.0,0.0,0.0 +0,1.892000,0.0,0.0,0.0,0.0,0.0 +0,1.892100,0.0,0.0,0.0,0.0,0.0 +0,1.892200,0.0,0.0,0.0,0.0,0.0 +0,1.892300,0.0,0.0,0.0,0.0,0.0 +0,1.892400,0.0,0.0,0.0,0.0,0.0 +0,1.892500,0.0,0.0,0.0,0.0,0.0 +0,1.892600,0.0,0.0,0.0,0.0,0.0 +0,1.892700,0.0,0.0,0.0,0.0,0.0 +0,1.892800,0.0,0.0,0.0,0.0,0.0 +0,1.892900,0.0,0.0,0.0,0.0,0.0 +0,1.893000,0.0,0.0,0.0,0.0,0.0 +0,1.893100,0.0,0.0,0.0,0.0,0.0 +0,1.893200,0.0,0.0,0.0,0.0,0.0 +0,1.893300,0.0,0.0,0.0,0.0,0.0 +0,1.893400,0.0,0.0,0.0,0.0,0.0 +0,1.893500,0.0,0.0,0.0,0.0,0.0 +0,1.893600,0.0,0.0,0.0,0.0,0.0 +0,1.893700,0.0,0.0,0.0,0.0,0.0 +0,1.893800,0.0,0.0,0.0,0.0,0.0 +0,1.893900,0.0,0.0,0.0,0.0,0.0 +0,1.894000,0.0,0.0,0.0,0.0,0.0 +0,1.894100,0.0,0.0,0.0,0.0,0.0 +0,1.894200,0.0,0.0,0.0,0.0,0.0 +0,1.894300,0.0,0.0,0.0,0.0,0.0 +0,1.894400,0.0,0.0,0.0,0.0,0.0 +0,1.894500,0.0,0.0,0.0,0.0,0.0 +0,1.894600,0.0,0.0,0.0,0.0,0.0 +0,1.894700,0.0,0.0,0.0,0.0,0.0 +0,1.894800,0.0,0.0,0.0,0.0,0.0 +0,1.894900,0.0,0.0,0.0,0.0,0.0 +0,1.895000,0.0,0.0,0.0,0.0,0.0 +0,1.895100,0.0,0.0,0.0,0.0,0.0 +0,1.895200,0.0,0.0,0.0,0.0,0.0 +0,1.895300,0.0,0.0,0.0,0.0,0.0 +0,1.895400,0.0,0.0,0.0,0.0,0.0 +0,1.895500,0.0,0.0,0.0,0.0,0.0 +0,1.895600,0.0,0.0,0.0,0.0,0.0 +0,1.895700,0.0,0.0,0.0,0.0,0.0 +0,1.895800,0.0,0.0,0.0,0.0,0.0 +0,1.895900,0.0,0.0,0.0,0.0,0.0 +0,1.896000,0.0,0.0,0.0,0.0,0.0 +0,1.896100,0.0,0.0,0.0,0.0,0.0 +0,1.896200,0.0,0.0,0.0,0.0,0.0 +0,1.896300,0.0,0.0,0.0,0.0,0.0 +0,1.896400,0.0,0.0,0.0,0.0,0.0 +0,1.896500,0.0,0.0,0.0,0.0,0.0 +0,1.896600,0.0,0.0,0.0,0.0,0.0 +0,1.896700,0.0,0.0,0.0,0.0,0.0 +0,1.896800,0.0,0.0,0.0,0.0,0.0 +0,1.896900,0.0,0.0,0.0,0.0,0.0 +0,1.897000,0.0,0.0,0.0,0.0,0.0 +0,1.897100,0.0,0.0,0.0,0.0,0.0 +0,1.897200,0.0,0.0,0.0,0.0,0.0 +0,1.897300,0.0,0.0,0.0,0.0,0.0 +0,1.897400,0.0,0.0,0.0,0.0,0.0 +0,1.897500,0.0,0.0,0.0,0.0,0.0 +0,1.897600,0.0,0.0,0.0,0.0,0.0 +0,1.897700,0.0,0.0,0.0,0.0,0.0 +0,1.897800,0.0,0.0,0.0,0.0,0.0 +0,1.897900,0.0,0.0,0.0,0.0,0.0 +0,1.898000,0.0,0.0,0.0,0.0,0.0 +0,1.898100,0.0,0.0,0.0,0.0,0.0 +0,1.898200,0.0,0.0,0.0,0.0,0.0 +0,1.898300,0.0,0.0,0.0,0.0,0.0 +0,1.898400,0.0,0.0,0.0,0.0,0.0 +0,1.898500,0.0,0.0,0.0,0.0,0.0 +0,1.898600,0.0,0.0,0.0,0.0,0.0 +0,1.898700,0.0,0.0,0.0,0.0,0.0 +0,1.898800,0.0,0.0,0.0,0.0,0.0 +0,1.898900,0.0,0.0,0.0,0.0,0.0 +0,1.899000,0.0,0.0,0.0,0.0,0.0 +0,1.899100,0.0,0.0,0.0,0.0,0.0 +0,1.899200,0.0,0.0,0.0,0.0,0.0 +0,1.899300,0.0,0.0,0.0,0.0,0.0 +0,1.899400,0.0,0.0,0.0,0.0,0.0 +0,1.899500,0.0,0.0,0.0,0.0,0.0 +0,1.899600,0.0,0.0,0.0,0.0,0.0 +0,1.899700,0.0,0.0,0.0,0.0,0.0 +0,1.899800,0.0,0.0,0.0,0.0,0.0 +0,1.899900,0.0,0.0,0.0,0.0,0.0 +0,1.900000,0.0,0.0,0.0,0.0,0.0 +0,1.900100,0.0,0.0,0.0,0.0,0.0 +1,2858.593593,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.900200,0.0,0.0,0.0,0.0,0.0 +0,1.900300,0.0,0.0,0.0,0.0,0.0 +0,1.900400,0.0,0.0,0.0,0.0,0.0 +0,1.900500,0.0,0.0,0.0,0.0,0.0 +0,1.900600,0.0,0.0,0.0,0.0,0.0 +0,1.900700,0.0,0.0,0.0,0.0,0.0 +0,1.900800,0.0,0.0,0.0,0.0,0.0 +0,1.900900,0.0,0.0,0.0,0.0,0.0 +0,1.901000,0.0,0.0,0.0,0.0,0.0 +0,1.901100,0.0,0.0,0.0,0.0,0.0 +0,1.901200,0.0,0.0,0.0,0.0,0.0 +0,1.901300,0.0,0.0,0.0,0.0,0.0 +0,1.901400,0.0,0.0,0.0,0.0,0.0 +0,1.901500,0.0,0.0,0.0,0.0,0.0 +0,1.901600,0.0,0.0,0.0,0.0,0.0 +0,1.901700,0.0,0.0,0.0,0.0,0.0 +0,1.901800,0.0,0.0,0.0,0.0,0.0 +0,1.901900,0.0,0.0,0.0,0.0,0.0 +0,1.902000,0.0,0.0,0.0,0.0,0.0 +0,1.902100,0.0,0.0,0.0,0.0,0.0 +0,1.902200,0.0,0.0,0.0,0.0,0.0 +0,1.902300,0.0,0.0,0.0,0.0,0.0 +0,1.902400,0.0,0.0,0.0,0.0,0.0 +0,1.902500,0.0,0.0,0.0,0.0,0.0 +0,1.902600,0.0,0.0,0.0,0.0,0.0 +0,1.902700,0.0,0.0,0.0,0.0,0.0 +0,1.902800,0.0,0.0,0.0,0.0,0.0 +0,1.902900,0.0,0.0,0.0,0.0,0.0 +0,1.903000,0.0,0.0,0.0,0.0,0.0 +0,1.903100,0.0,0.0,0.0,0.0,0.0 +0,1.903200,0.0,0.0,0.0,0.0,0.0 +0,1.903300,0.0,0.0,0.0,0.0,0.0 +0,1.903400,0.0,0.0,0.0,0.0,0.0 +0,1.903500,0.0,0.0,0.0,0.0,0.0 +0,1.903600,0.0,0.0,0.0,0.0,0.0 +0,1.903700,0.0,0.0,0.0,0.0,0.0 +0,1.903800,0.0,0.0,0.0,0.0,0.0 +0,1.903900,0.0,0.0,0.0,0.0,0.0 +0,1.904000,0.0,0.0,0.0,0.0,0.0 +0,1.904100,0.0,0.0,0.0,0.0,0.0 +0,1.904200,0.0,0.0,0.0,0.0,0.0 +0,1.904300,0.0,0.0,0.0,0.0,0.0 +0,1.904400,0.0,0.0,0.0,0.0,0.0 +0,1.904500,0.0,0.0,0.0,0.0,0.0 +0,1.904600,0.0,0.0,0.0,0.0,0.0 +0,1.904700,0.0,0.0,0.0,0.0,0.0 +0,1.904800,0.0,0.0,0.0,0.0,0.0 +0,1.904900,0.0,0.0,0.0,0.0,0.0 +0,1.905000,0.0,0.0,0.0,0.0,0.0 +0,1.905100,0.0,0.0,0.0,0.0,0.0 +0,1.905200,0.0,0.0,0.0,0.0,0.0 +0,1.905300,0.0,0.0,0.0,0.0,0.0 +0,1.905400,0.0,0.0,0.0,0.0,0.0 +0,1.905500,0.0,0.0,0.0,0.0,0.0 +0,1.905600,0.0,0.0,0.0,0.0,0.0 +0,1.905700,0.0,0.0,0.0,0.0,0.0 +0,1.905800,0.0,0.0,0.0,0.0,0.0 +0,1.905900,0.0,0.0,0.0,0.0,0.0 +0,1.906000,0.0,0.0,0.0,0.0,0.0 +0,1.906100,0.0,0.0,0.0,0.0,0.0 +0,1.906200,0.0,0.0,0.0,0.0,0.0 +0,1.906300,0.0,0.0,0.0,0.0,0.0 +0,1.906400,0.0,0.0,0.0,0.0,0.0 +0,1.906500,0.0,0.0,0.0,0.0,0.0 +0,1.906600,0.0,0.0,0.0,0.0,0.0 +0,1.906700,0.0,0.0,0.0,0.0,0.0 +0,1.906800,0.0,0.0,0.0,0.0,0.0 +0,1.906900,0.0,0.0,0.0,0.0,0.0 +0,1.907000,0.0,0.0,0.0,0.0,0.0 +0,1.907100,0.0,0.0,0.0,0.0,0.0 +0,1.907200,0.0,0.0,0.0,0.0,0.0 +0,1.907300,0.0,0.0,0.0,0.0,0.0 +0,1.907400,0.0,0.0,0.0,0.0,0.0 +0,1.907500,0.0,0.0,0.0,0.0,0.0 +0,1.907600,0.0,0.0,0.0,0.0,0.0 +0,1.907700,0.0,0.0,0.0,0.0,0.0 +0,1.907800,0.0,0.0,0.0,0.0,0.0 +0,1.907900,0.0,0.0,0.0,0.0,0.0 +0,1.908000,0.0,0.0,0.0,0.0,0.0 +0,1.908100,0.0,0.0,0.0,0.0,0.0 +0,1.908200,0.0,0.0,0.0,0.0,0.0 +0,1.908300,0.0,0.0,0.0,0.0,0.0 +0,1.908400,0.0,0.0,0.0,0.0,0.0 +0,1.908500,0.0,0.0,0.0,0.0,0.0 +0,1.908600,0.0,0.0,0.0,0.0,0.0 +0,1.908700,0.0,0.0,0.0,0.0,0.0 +0,1.908800,0.0,0.0,0.0,0.0,0.0 +0,1.908900,0.0,0.0,0.0,0.0,0.0 +0,1.909000,0.0,0.0,0.0,0.0,0.0 +0,1.909100,0.0,0.0,0.0,0.0,0.0 +0,1.909200,0.0,0.0,0.0,0.0,0.0 +0,1.909300,0.0,0.0,0.0,0.0,0.0 +0,1.909400,0.0,0.0,0.0,0.0,0.0 +0,1.909500,0.0,0.0,0.0,0.0,0.0 +0,1.909600,0.0,0.0,0.0,0.0,0.0 +0,1.909700,0.0,0.0,0.0,0.0,0.0 +0,1.909800,0.0,0.0,0.0,0.0,0.0 +0,1.909900,0.0,0.0,0.0,0.0,0.0 +0,1.910000,0.0,0.0,0.0,0.0,0.0 +0,1.910100,0.0,0.0,0.0,0.0,0.0 +0,1.910200,0.0,0.0,0.0,0.0,0.0 +0,1.910300,0.0,0.0,0.0,0.0,0.0 +0,1.910400,0.0,0.0,0.0,0.0,0.0 +0,1.910500,0.0,0.0,0.0,0.0,0.0 +0,1.910600,0.0,0.0,0.0,0.0,0.0 +0,1.910700,0.0,0.0,0.0,0.0,0.0 +0,1.910800,0.0,0.0,0.0,0.0,0.0 +0,1.910900,0.0,0.0,0.0,0.0,0.0 +0,1.911000,0.0,0.0,0.0,0.0,0.0 +0,1.911100,0.0,0.0,0.0,0.0,0.0 +0,1.911200,0.0,0.0,0.0,0.0,0.0 +0,1.911300,0.0,0.0,0.0,0.0,0.0 +0,1.911400,0.0,0.0,0.0,0.0,0.0 +0,1.911500,0.0,0.0,0.0,0.0,0.0 +0,1.911600,0.0,0.0,0.0,0.0,0.0 +0,1.911700,0.0,0.0,0.0,0.0,0.0 +0,1.911800,0.0,0.0,0.0,0.0,0.0 +0,1.911900,0.0,0.0,0.0,0.0,0.0 +0,1.912000,0.0,0.0,0.0,0.0,0.0 +0,1.912100,0.0,0.0,0.0,0.0,0.0 +0,1.912200,0.0,0.0,0.0,0.0,0.0 +0,1.912300,0.0,0.0,0.0,0.0,0.0 +0,1.912400,0.0,0.0,0.0,0.0,0.0 +0,1.912500,0.0,0.0,0.0,0.0,0.0 +0,1.912600,0.0,0.0,0.0,0.0,0.0 +0,1.912700,0.0,0.0,0.0,0.0,0.0 +0,1.912800,0.0,0.0,0.0,0.0,0.0 +0,1.912900,0.0,0.0,0.0,0.0,0.0 +0,1.913000,0.0,0.0,0.0,0.0,0.0 +0,1.913100,0.0,0.0,0.0,0.0,0.0 +0,1.913200,0.0,0.0,0.0,0.0,0.0 +0,1.913300,0.0,0.0,0.0,0.0,0.0 +0,1.913400,0.0,0.0,0.0,0.0,0.0 +0,1.913500,0.0,0.0,0.0,0.0,0.0 +0,1.913600,0.0,0.0,0.0,0.0,0.0 +0,1.913700,0.0,0.0,0.0,0.0,0.0 +0,1.913800,0.0,0.0,0.0,0.0,0.0 +0,1.913900,0.0,0.0,0.0,0.0,0.0 +0,1.914000,0.0,0.0,0.0,0.0,0.0 +0,1.914100,0.0,0.0,0.0,0.0,0.0 +0,1.914200,0.0,0.0,0.0,0.0,0.0 +0,1.914300,0.0,0.0,0.0,0.0,0.0 +0,1.914400,0.0,0.0,0.0,0.0,0.0 +0,1.914500,0.0,0.0,0.0,0.0,0.0 +0,1.914600,0.0,0.0,0.0,0.0,0.0 +0,1.914700,0.0,0.0,0.0,0.0,0.0 +0,1.914800,0.0,0.0,0.0,0.0,0.0 +0,1.914900,0.0,0.0,0.0,0.0,0.0 +0,1.915000,0.0,0.0,0.0,0.0,0.0 +0,1.915100,0.0,0.0,0.0,0.0,0.0 +0,1.915200,0.0,0.0,0.0,0.0,0.0 +0,1.915300,0.0,0.0,0.0,0.0,0.0 +0,1.915400,0.0,0.0,0.0,0.0,0.0 +0,1.915500,0.0,0.0,0.0,0.0,0.0 +0,1.915600,0.0,0.0,0.0,0.0,0.0 +0,1.915700,0.0,0.0,0.0,0.0,0.0 +0,1.915800,0.0,0.0,0.0,0.0,0.0 +0,1.915900,0.0,0.0,0.0,0.0,0.0 +0,1.916000,0.0,0.0,0.0,0.0,0.0 +0,1.916100,0.0,0.0,0.0,0.0,0.0 +0,1.916200,0.0,0.0,0.0,0.0,0.0 +0,1.916300,0.0,0.0,0.0,0.0,0.0 +0,1.916400,0.0,0.0,0.0,0.0,0.0 +0,1.916500,0.0,0.0,0.0,0.0,0.0 +0,1.916600,0.0,0.0,0.0,0.0,0.0 +0,1.916700,0.0,0.0,0.0,0.0,0.0 +0,1.916800,0.0,0.0,0.0,0.0,0.0 +0,1.916900,0.0,0.0,0.0,0.0,0.0 +0,1.917000,0.0,0.0,0.0,0.0,0.0 +0,1.917100,0.0,0.0,0.0,0.0,0.0 +0,1.917200,0.0,0.0,0.0,0.0,0.0 +0,1.917300,0.0,0.0,0.0,0.0,0.0 +0,1.917400,0.0,0.0,0.0,0.0,0.0 +0,1.917500,0.0,0.0,0.0,0.0,0.0 +0,1.917600,0.0,0.0,0.0,0.0,0.0 +0,1.917700,0.0,0.0,0.0,0.0,0.0 +0,1.917800,0.0,0.0,0.0,0.0,0.0 +0,1.917900,0.0,0.0,0.0,0.0,0.0 +0,1.918000,0.0,0.0,0.0,0.0,0.0 +0,1.918100,0.0,0.0,0.0,0.0,0.0 +0,1.918200,0.0,0.0,0.0,0.0,0.0 +0,1.918300,0.0,0.0,0.0,0.0,0.0 +0,1.918400,0.0,0.0,0.0,0.0,0.0 +0,1.918500,0.0,0.0,0.0,0.0,0.0 +0,1.918600,0.0,0.0,0.0,0.0,0.0 +0,1.918700,0.0,0.0,0.0,0.0,0.0 +0,1.918800,0.0,0.0,0.0,0.0,0.0 +0,1.918900,0.0,0.0,0.0,0.0,0.0 +0,1.919000,0.0,0.0,0.0,0.0,0.0 +0,1.919100,0.0,0.0,0.0,0.0,0.0 +0,1.919200,0.0,0.0,0.0,0.0,0.0 +0,1.919300,0.0,0.0,0.0,0.0,0.0 +0,1.919400,0.0,0.0,0.0,0.0,0.0 +0,1.919500,0.0,0.0,0.0,0.0,0.0 +0,1.919600,0.0,0.0,0.0,0.0,0.0 +0,1.919700,0.0,0.0,0.0,0.0,0.0 +0,1.919800,0.0,0.0,0.0,0.0,0.0 +0,1.919900,0.0,0.0,0.0,0.0,0.0 +0,1.920000,0.0,0.0,0.0,0.0,0.0 +0,1.920100,0.0,0.0,0.0,0.0,0.0 +1,2949.811252,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.920200,0.0,0.0,0.0,0.0,0.0 +0,1.920300,0.0,0.0,0.0,0.0,0.0 +0,1.920400,0.0,0.0,0.0,0.0,0.0 +0,1.920500,0.0,0.0,0.0,0.0,0.0 +0,1.920600,0.0,0.0,0.0,0.0,0.0 +0,1.920700,0.0,0.0,0.0,0.0,0.0 +0,1.920800,0.0,0.0,0.0,0.0,0.0 +0,1.920900,0.0,0.0,0.0,0.0,0.0 +0,1.921000,0.0,0.0,0.0,0.0,0.0 +0,1.921100,0.0,0.0,0.0,0.0,0.0 +0,1.921200,0.0,0.0,0.0,0.0,0.0 +0,1.921300,0.0,0.0,0.0,0.0,0.0 +0,1.921400,0.0,0.0,0.0,0.0,0.0 +0,1.921500,0.0,0.0,0.0,0.0,0.0 +0,1.921600,0.0,0.0,0.0,0.0,0.0 +0,1.921700,0.0,0.0,0.0,0.0,0.0 +0,1.921800,0.0,0.0,0.0,0.0,0.0 +0,1.921900,0.0,0.0,0.0,0.0,0.0 +0,1.922000,0.0,0.0,0.0,0.0,0.0 +0,1.922100,0.0,0.0,0.0,0.0,0.0 +0,1.922200,0.0,0.0,0.0,0.0,0.0 +0,1.922300,0.0,0.0,0.0,0.0,0.0 +0,1.922400,0.0,0.0,0.0,0.0,0.0 +0,1.922500,0.0,0.0,0.0,0.0,0.0 +0,1.922600,0.0,0.0,0.0,0.0,0.0 +0,1.922700,0.0,0.0,0.0,0.0,0.0 +0,1.922800,0.0,0.0,0.0,0.0,0.0 +0,1.922900,0.0,0.0,0.0,0.0,0.0 +0,1.923000,0.0,0.0,0.0,0.0,0.0 +0,1.923100,0.0,0.0,0.0,0.0,0.0 +0,1.923200,0.0,0.0,0.0,0.0,0.0 +0,1.923300,0.0,0.0,0.0,0.0,0.0 +0,1.923400,0.0,0.0,0.0,0.0,0.0 +0,1.923500,0.0,0.0,0.0,0.0,0.0 +0,1.923600,0.0,0.0,0.0,0.0,0.0 +0,1.923700,0.0,0.0,0.0,0.0,0.0 +0,1.923800,0.0,0.0,0.0,0.0,0.0 +0,1.923900,0.0,0.0,0.0,0.0,0.0 +0,1.924000,0.0,0.0,0.0,0.0,0.0 +0,1.924100,0.0,0.0,0.0,0.0,0.0 +0,1.924200,0.0,0.0,0.0,0.0,0.0 +0,1.924300,0.0,0.0,0.0,0.0,0.0 +0,1.924400,0.0,0.0,0.0,0.0,0.0 +0,1.924500,0.0,0.0,0.0,0.0,0.0 +0,1.924600,0.0,0.0,0.0,0.0,0.0 +0,1.924700,0.0,0.0,0.0,0.0,0.0 +0,1.924800,0.0,0.0,0.0,0.0,0.0 +0,1.924900,0.0,0.0,0.0,0.0,0.0 +0,1.925000,0.0,0.0,0.0,0.0,0.0 +0,1.925100,0.0,0.0,0.0,0.0,0.0 +0,1.925200,0.0,0.0,0.0,0.0,0.0 +0,1.925300,0.0,0.0,0.0,0.0,0.0 +0,1.925400,0.0,0.0,0.0,0.0,0.0 +0,1.925500,0.0,0.0,0.0,0.0,0.0 +0,1.925600,0.0,0.0,0.0,0.0,0.0 +0,1.925700,0.0,0.0,0.0,0.0,0.0 +0,1.925800,0.0,0.0,0.0,0.0,0.0 +0,1.925900,0.0,0.0,0.0,0.0,0.0 +0,1.926000,0.0,0.0,0.0,0.0,0.0 +0,1.926100,0.0,0.0,0.0,0.0,0.0 +0,1.926200,0.0,0.0,0.0,0.0,0.0 +0,1.926300,0.0,0.0,0.0,0.0,0.0 +0,1.926400,0.0,0.0,0.0,0.0,0.0 +0,1.926500,0.0,0.0,0.0,0.0,0.0 +0,1.926600,0.0,0.0,0.0,0.0,0.0 +0,1.926700,0.0,0.0,0.0,0.0,0.0 +0,1.926800,0.0,0.0,0.0,0.0,0.0 +0,1.926900,0.0,0.0,0.0,0.0,0.0 +0,1.927000,0.0,0.0,0.0,0.0,0.0 +0,1.927100,0.0,0.0,0.0,0.0,0.0 +0,1.927200,0.0,0.0,0.0,0.0,0.0 +0,1.927300,0.0,0.0,0.0,0.0,0.0 +0,1.927400,0.0,0.0,0.0,0.0,0.0 +0,1.927500,0.0,0.0,0.0,0.0,0.0 +0,1.927600,0.0,0.0,0.0,0.0,0.0 +0,1.927700,0.0,0.0,0.0,0.0,0.0 +0,1.927800,0.0,0.0,0.0,0.0,0.0 +0,1.927900,0.0,0.0,0.0,0.0,0.0 +0,1.928000,0.0,0.0,0.0,0.0,0.0 +0,1.928100,0.0,0.0,0.0,0.0,0.0 +0,1.928200,0.0,0.0,0.0,0.0,0.0 +0,1.928300,0.0,0.0,0.0,0.0,0.0 +0,1.928400,0.0,0.0,0.0,0.0,0.0 +0,1.928500,0.0,0.0,0.0,0.0,0.0 +0,1.928600,0.0,0.0,0.0,0.0,0.0 +0,1.928700,0.0,0.0,0.0,0.0,0.0 +0,1.928800,0.0,0.0,0.0,0.0,0.0 +0,1.928900,0.0,0.0,0.0,0.0,0.0 +0,1.929000,0.0,0.0,0.0,0.0,0.0 +0,1.929100,0.0,0.0,0.0,0.0,0.0 +0,1.929200,0.0,0.0,0.0,0.0,0.0 +0,1.929300,0.0,0.0,0.0,0.0,0.0 +0,1.929400,0.0,0.0,0.0,0.0,0.0 +0,1.929500,0.0,0.0,0.0,0.0,0.0 +0,1.929600,0.0,0.0,0.0,0.0,0.0 +0,1.929700,0.0,0.0,0.0,0.0,0.0 +0,1.929800,0.0,0.0,0.0,0.0,0.0 +0,1.929900,0.0,0.0,0.0,0.0,0.0 +0,1.930000,0.0,0.0,0.0,0.0,0.0 +0,1.930100,0.0,0.0,0.0,0.0,0.0 +0,1.930200,0.0,0.0,0.0,0.0,0.0 +0,1.930300,0.0,0.0,0.0,0.0,0.0 +0,1.930400,0.0,0.0,0.0,0.0,0.0 +0,1.930500,0.0,0.0,0.0,0.0,0.0 +0,1.930600,0.0,0.0,0.0,0.0,0.0 +0,1.930700,0.0,0.0,0.0,0.0,0.0 +0,1.930800,0.0,0.0,0.0,0.0,0.0 +0,1.930900,0.0,0.0,0.0,0.0,0.0 +0,1.931000,0.0,0.0,0.0,0.0,0.0 +0,1.931100,0.0,0.0,0.0,0.0,0.0 +0,1.931200,0.0,0.0,0.0,0.0,0.0 +0,1.931300,0.0,0.0,0.0,0.0,0.0 +0,1.931400,0.0,0.0,0.0,0.0,0.0 +0,1.931500,0.0,0.0,0.0,0.0,0.0 +0,1.931600,0.0,0.0,0.0,0.0,0.0 +0,1.931700,0.0,0.0,0.0,0.0,0.0 +0,1.931800,0.0,0.0,0.0,0.0,0.0 +0,1.931900,0.0,0.0,0.0,0.0,0.0 +0,1.932000,0.0,0.0,0.0,0.0,0.0 +0,1.932100,0.0,0.0,0.0,0.0,0.0 +0,1.932200,0.0,0.0,0.0,0.0,0.0 +0,1.932300,0.0,0.0,0.0,0.0,0.0 +0,1.932400,0.0,0.0,0.0,0.0,0.0 +0,1.932500,0.0,0.0,0.0,0.0,0.0 +0,1.932600,0.0,0.0,0.0,0.0,0.0 +0,1.932700,0.0,0.0,0.0,0.0,0.0 +0,1.932800,0.0,0.0,0.0,0.0,0.0 +0,1.932900,0.0,0.0,0.0,0.0,0.0 +0,1.933000,0.0,0.0,0.0,0.0,0.0 +0,1.933100,0.0,0.0,0.0,0.0,0.0 +0,1.933200,0.0,0.0,0.0,0.0,0.0 +0,1.933300,0.0,0.0,0.0,0.0,0.0 +0,1.933400,0.0,0.0,0.0,0.0,0.0 +0,1.933500,0.0,0.0,0.0,0.0,0.0 +0,1.933600,0.0,0.0,0.0,0.0,0.0 +0,1.933700,0.0,0.0,0.0,0.0,0.0 +0,1.933800,0.0,0.0,0.0,0.0,0.0 +0,1.933900,0.0,0.0,0.0,0.0,0.0 +0,1.934000,0.0,0.0,0.0,0.0,0.0 +0,1.934100,0.0,0.0,0.0,0.0,0.0 +0,1.934200,0.0,0.0,0.0,0.0,0.0 +0,1.934300,0.0,0.0,0.0,0.0,0.0 +0,1.934400,0.0,0.0,0.0,0.0,0.0 +0,1.934500,0.0,0.0,0.0,0.0,0.0 +0,1.934600,0.0,0.0,0.0,0.0,0.0 +0,1.934700,0.0,0.0,0.0,0.0,0.0 +0,1.934800,0.0,0.0,0.0,0.0,0.0 +0,1.934900,0.0,0.0,0.0,0.0,0.0 +0,1.935000,0.0,0.0,0.0,0.0,0.0 +0,1.935100,0.0,0.0,0.0,0.0,0.0 +0,1.935200,0.0,0.0,0.0,0.0,0.0 +0,1.935300,0.0,0.0,0.0,0.0,0.0 +0,1.935400,0.0,0.0,0.0,0.0,0.0 +0,1.935500,0.0,0.0,0.0,0.0,0.0 +0,1.935600,0.0,0.0,0.0,0.0,0.0 +0,1.935700,0.0,0.0,0.0,0.0,0.0 +0,1.935800,0.0,0.0,0.0,0.0,0.0 +0,1.935900,0.0,0.0,0.0,0.0,0.0 +0,1.936000,0.0,0.0,0.0,0.0,0.0 +0,1.936100,0.0,0.0,0.0,0.0,0.0 +0,1.936200,0.0,0.0,0.0,0.0,0.0 +0,1.936300,0.0,0.0,0.0,0.0,0.0 +0,1.936400,0.0,0.0,0.0,0.0,0.0 +0,1.936500,0.0,0.0,0.0,0.0,0.0 +0,1.936600,0.0,0.0,0.0,0.0,0.0 +0,1.936700,0.0,0.0,0.0,0.0,0.0 +0,1.936800,0.0,0.0,0.0,0.0,0.0 +0,1.936900,0.0,0.0,0.0,0.0,0.0 +0,1.937000,0.0,0.0,0.0,0.0,0.0 +0,1.937100,0.0,0.0,0.0,0.0,0.0 +0,1.937200,0.0,0.0,0.0,0.0,0.0 +0,1.937300,0.0,0.0,0.0,0.0,0.0 +0,1.937400,0.0,0.0,0.0,0.0,0.0 +0,1.937500,0.0,0.0,0.0,0.0,0.0 +0,1.937600,0.0,0.0,0.0,0.0,0.0 +0,1.937700,0.0,0.0,0.0,0.0,0.0 +0,1.937800,0.0,0.0,0.0,0.0,0.0 +0,1.937900,0.0,0.0,0.0,0.0,0.0 +0,1.938000,0.0,0.0,0.0,0.0,0.0 +0,1.938100,0.0,0.0,0.0,0.0,0.0 +0,1.938200,0.0,0.0,0.0,0.0,0.0 +0,1.938300,0.0,0.0,0.0,0.0,0.0 +0,1.938400,0.0,0.0,0.0,0.0,0.0 +0,1.938500,0.0,0.0,0.0,0.0,0.0 +0,1.938600,0.0,0.0,0.0,0.0,0.0 +0,1.938700,0.0,0.0,0.0,0.0,0.0 +0,1.938800,0.0,0.0,0.0,0.0,0.0 +0,1.938900,0.0,0.0,0.0,0.0,0.0 +0,1.939000,0.0,0.0,0.0,0.0,0.0 +0,1.939100,0.0,0.0,0.0,0.0,0.0 +0,1.939200,0.0,0.0,0.0,0.0,0.0 +0,1.939300,0.0,0.0,0.0,0.0,0.0 +0,1.939400,0.0,0.0,0.0,0.0,0.0 +0,1.939500,0.0,0.0,0.0,0.0,0.0 +0,1.939600,0.0,0.0,0.0,0.0,0.0 +0,1.939700,0.0,0.0,0.0,0.0,0.0 +0,1.939800,0.0,0.0,0.0,0.0,0.0 +0,1.939900,0.0,0.0,0.0,0.0,0.0 +0,1.940000,0.0,0.0,0.0,0.0,0.0 +0,1.940100,0.0,0.0,0.0,0.0,0.0 +1,3042.949061,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.940200,0.0,0.0,0.0,0.0,0.0 +0,1.940300,0.0,0.0,0.0,0.0,0.0 +0,1.940400,0.0,0.0,0.0,0.0,0.0 +0,1.940500,0.0,0.0,0.0,0.0,0.0 +0,1.940600,0.0,0.0,0.0,0.0,0.0 +0,1.940700,0.0,0.0,0.0,0.0,0.0 +0,1.940800,0.0,0.0,0.0,0.0,0.0 +0,1.940900,0.0,0.0,0.0,0.0,0.0 +0,1.941000,0.0,0.0,0.0,0.0,0.0 +0,1.941100,0.0,0.0,0.0,0.0,0.0 +0,1.941200,0.0,0.0,0.0,0.0,0.0 +0,1.941300,0.0,0.0,0.0,0.0,0.0 +0,1.941400,0.0,0.0,0.0,0.0,0.0 +0,1.941500,0.0,0.0,0.0,0.0,0.0 +0,1.941600,0.0,0.0,0.0,0.0,0.0 +0,1.941700,0.0,0.0,0.0,0.0,0.0 +0,1.941800,0.0,0.0,0.0,0.0,0.0 +0,1.941900,0.0,0.0,0.0,0.0,0.0 +0,1.942000,0.0,0.0,0.0,0.0,0.0 +0,1.942100,0.0,0.0,0.0,0.0,0.0 +0,1.942200,0.0,0.0,0.0,0.0,0.0 +0,1.942300,0.0,0.0,0.0,0.0,0.0 +0,1.942400,0.0,0.0,0.0,0.0,0.0 +0,1.942500,0.0,0.0,0.0,0.0,0.0 +0,1.942600,0.0,0.0,0.0,0.0,0.0 +0,1.942700,0.0,0.0,0.0,0.0,0.0 +0,1.942800,0.0,0.0,0.0,0.0,0.0 +0,1.942900,0.0,0.0,0.0,0.0,0.0 +0,1.943000,0.0,0.0,0.0,0.0,0.0 +0,1.943100,0.0,0.0,0.0,0.0,0.0 +0,1.943200,0.0,0.0,0.0,0.0,0.0 +0,1.943300,0.0,0.0,0.0,0.0,0.0 +0,1.943400,0.0,0.0,0.0,0.0,0.0 +0,1.943500,0.0,0.0,0.0,0.0,0.0 +0,1.943600,0.0,0.0,0.0,0.0,0.0 +0,1.943700,0.0,0.0,0.0,0.0,0.0 +0,1.943800,0.0,0.0,0.0,0.0,0.0 +0,1.943900,0.0,0.0,0.0,0.0,0.0 +0,1.944000,0.0,0.0,0.0,0.0,0.0 +0,1.944100,0.0,0.0,0.0,0.0,0.0 +0,1.944200,0.0,0.0,0.0,0.0,0.0 +0,1.944300,0.0,0.0,0.0,0.0,0.0 +0,1.944400,0.0,0.0,0.0,0.0,0.0 +0,1.944500,0.0,0.0,0.0,0.0,0.0 +0,1.944600,0.0,0.0,0.0,0.0,0.0 +0,1.944700,0.0,0.0,0.0,0.0,0.0 +0,1.944800,0.0,0.0,0.0,0.0,0.0 +0,1.944900,0.0,0.0,0.0,0.0,0.0 +0,1.945000,0.0,0.0,0.0,0.0,0.0 +0,1.945100,0.0,0.0,0.0,0.0,0.0 +0,1.945200,0.0,0.0,0.0,0.0,0.0 +0,1.945300,0.0,0.0,0.0,0.0,0.0 +0,1.945400,0.0,0.0,0.0,0.0,0.0 +0,1.945500,0.0,0.0,0.0,0.0,0.0 +0,1.945600,0.0,0.0,0.0,0.0,0.0 +0,1.945700,0.0,0.0,0.0,0.0,0.0 +0,1.945800,0.0,0.0,0.0,0.0,0.0 +0,1.945900,0.0,0.0,0.0,0.0,0.0 +0,1.946000,0.0,0.0,0.0,0.0,0.0 +0,1.946100,0.0,0.0,0.0,0.0,0.0 +0,1.946200,0.0,0.0,0.0,0.0,0.0 +0,1.946300,0.0,0.0,0.0,0.0,0.0 +0,1.946400,0.0,0.0,0.0,0.0,0.0 +0,1.946500,0.0,0.0,0.0,0.0,0.0 +0,1.946600,0.0,0.0,0.0,0.0,0.0 +0,1.946700,0.0,0.0,0.0,0.0,0.0 +0,1.946800,0.0,0.0,0.0,0.0,0.0 +0,1.946900,0.0,0.0,0.0,0.0,0.0 +0,1.947000,0.0,0.0,0.0,0.0,0.0 +0,1.947100,0.0,0.0,0.0,0.0,0.0 +0,1.947200,0.0,0.0,0.0,0.0,0.0 +0,1.947300,0.0,0.0,0.0,0.0,0.0 +0,1.947400,0.0,0.0,0.0,0.0,0.0 +0,1.947500,0.0,0.0,0.0,0.0,0.0 +0,1.947600,0.0,0.0,0.0,0.0,0.0 +0,1.947700,0.0,0.0,0.0,0.0,0.0 +0,1.947800,0.0,0.0,0.0,0.0,0.0 +0,1.947900,0.0,0.0,0.0,0.0,0.0 +0,1.948000,0.0,0.0,0.0,0.0,0.0 +0,1.948100,0.0,0.0,0.0,0.0,0.0 +0,1.948200,0.0,0.0,0.0,0.0,0.0 +0,1.948300,0.0,0.0,0.0,0.0,0.0 +0,1.948400,0.0,0.0,0.0,0.0,0.0 +0,1.948500,0.0,0.0,0.0,0.0,0.0 +0,1.948600,0.0,0.0,0.0,0.0,0.0 +0,1.948700,0.0,0.0,0.0,0.0,0.0 +0,1.948800,0.0,0.0,0.0,0.0,0.0 +0,1.948900,0.0,0.0,0.0,0.0,0.0 +0,1.949000,0.0,0.0,0.0,0.0,0.0 +0,1.949100,0.0,0.0,0.0,0.0,0.0 +0,1.949200,0.0,0.0,0.0,0.0,0.0 +0,1.949300,0.0,0.0,0.0,0.0,0.0 +0,1.949400,0.0,0.0,0.0,0.0,0.0 +0,1.949500,0.0,0.0,0.0,0.0,0.0 +0,1.949600,0.0,0.0,0.0,0.0,0.0 +0,1.949700,0.0,0.0,0.0,0.0,0.0 +0,1.949800,0.0,0.0,0.0,0.0,0.0 +0,1.949900,0.0,0.0,0.0,0.0,0.0 +0,1.950000,0.0,0.0,0.0,0.0,0.0 +0,1.950100,0.0,0.0,0.0,0.0,0.0 +0,1.950200,0.0,0.0,0.0,0.0,0.0 +0,1.950300,0.0,0.0,0.0,0.0,0.0 +0,1.950400,0.0,0.0,0.0,0.0,0.0 +0,1.950500,0.0,0.0,0.0,0.0,0.0 +0,1.950600,0.0,0.0,0.0,0.0,0.0 +0,1.950700,0.0,0.0,0.0,0.0,0.0 +0,1.950800,0.0,0.0,0.0,0.0,0.0 +0,1.950900,0.0,0.0,0.0,0.0,0.0 +0,1.951000,0.0,0.0,0.0,0.0,0.0 +0,1.951100,0.0,0.0,0.0,0.0,0.0 +0,1.951200,0.0,0.0,0.0,0.0,0.0 +0,1.951300,0.0,0.0,0.0,0.0,0.0 +0,1.951400,0.0,0.0,0.0,0.0,0.0 +0,1.951500,0.0,0.0,0.0,0.0,0.0 +0,1.951600,0.0,0.0,0.0,0.0,0.0 +0,1.951700,0.0,0.0,0.0,0.0,0.0 +0,1.951800,0.0,0.0,0.0,0.0,0.0 +0,1.951900,0.0,0.0,0.0,0.0,0.0 +0,1.952000,0.0,0.0,0.0,0.0,0.0 +0,1.952100,0.0,0.0,0.0,0.0,0.0 +0,1.952200,0.0,0.0,0.0,0.0,0.0 +0,1.952300,0.0,0.0,0.0,0.0,0.0 +0,1.952400,0.0,0.0,0.0,0.0,0.0 +0,1.952500,0.0,0.0,0.0,0.0,0.0 +0,1.952600,0.0,0.0,0.0,0.0,0.0 +0,1.952700,0.0,0.0,0.0,0.0,0.0 +0,1.952800,0.0,0.0,0.0,0.0,0.0 +0,1.952900,0.0,0.0,0.0,0.0,0.0 +0,1.953000,0.0,0.0,0.0,0.0,0.0 +0,1.953100,0.0,0.0,0.0,0.0,0.0 +0,1.953200,0.0,0.0,0.0,0.0,0.0 +0,1.953300,0.0,0.0,0.0,0.0,0.0 +0,1.953400,0.0,0.0,0.0,0.0,0.0 +0,1.953500,0.0,0.0,0.0,0.0,0.0 +0,1.953600,0.0,0.0,0.0,0.0,0.0 +0,1.953700,0.0,0.0,0.0,0.0,0.0 +0,1.953800,0.0,0.0,0.0,0.0,0.0 +0,1.953900,0.0,0.0,0.0,0.0,0.0 +0,1.954000,0.0,0.0,0.0,0.0,0.0 +0,1.954100,0.0,0.0,0.0,0.0,0.0 +0,1.954200,0.0,0.0,0.0,0.0,0.0 +0,1.954300,0.0,0.0,0.0,0.0,0.0 +0,1.954400,0.0,0.0,0.0,0.0,0.0 +0,1.954500,0.0,0.0,0.0,0.0,0.0 +0,1.954600,0.0,0.0,0.0,0.0,0.0 +0,1.954700,0.0,0.0,0.0,0.0,0.0 +0,1.954800,0.0,0.0,0.0,0.0,0.0 +0,1.954900,0.0,0.0,0.0,0.0,0.0 +0,1.955000,0.0,0.0,0.0,0.0,0.0 +0,1.955100,0.0,0.0,0.0,0.0,0.0 +0,1.955200,0.0,0.0,0.0,0.0,0.0 +0,1.955300,0.0,0.0,0.0,0.0,0.0 +0,1.955400,0.0,0.0,0.0,0.0,0.0 +0,1.955500,0.0,0.0,0.0,0.0,0.0 +0,1.955600,0.0,0.0,0.0,0.0,0.0 +0,1.955700,0.0,0.0,0.0,0.0,0.0 +0,1.955800,0.0,0.0,0.0,0.0,0.0 +0,1.955900,0.0,0.0,0.0,0.0,0.0 +0,1.956000,0.0,0.0,0.0,0.0,0.0 +0,1.956100,0.0,0.0,0.0,0.0,0.0 +0,1.956200,0.0,0.0,0.0,0.0,0.0 +0,1.956300,0.0,0.0,0.0,0.0,0.0 +0,1.956400,0.0,0.0,0.0,0.0,0.0 +0,1.956500,0.0,0.0,0.0,0.0,0.0 +0,1.956600,0.0,0.0,0.0,0.0,0.0 +0,1.956700,0.0,0.0,0.0,0.0,0.0 +0,1.956800,0.0,0.0,0.0,0.0,0.0 +0,1.956900,0.0,0.0,0.0,0.0,0.0 +0,1.957000,0.0,0.0,0.0,0.0,0.0 +0,1.957100,0.0,0.0,0.0,0.0,0.0 +0,1.957200,0.0,0.0,0.0,0.0,0.0 +0,1.957300,0.0,0.0,0.0,0.0,0.0 +0,1.957400,0.0,0.0,0.0,0.0,0.0 +0,1.957500,0.0,0.0,0.0,0.0,0.0 +0,1.957600,0.0,0.0,0.0,0.0,0.0 +0,1.957700,0.0,0.0,0.0,0.0,0.0 +0,1.957800,0.0,0.0,0.0,0.0,0.0 +0,1.957900,0.0,0.0,0.0,0.0,0.0 +0,1.958000,0.0,0.0,0.0,0.0,0.0 +0,1.958100,0.0,0.0,0.0,0.0,0.0 +0,1.958200,0.0,0.0,0.0,0.0,0.0 +0,1.958300,0.0,0.0,0.0,0.0,0.0 +0,1.958400,0.0,0.0,0.0,0.0,0.0 +0,1.958500,0.0,0.0,0.0,0.0,0.0 +0,1.958600,0.0,0.0,0.0,0.0,0.0 +0,1.958700,0.0,0.0,0.0,0.0,0.0 +0,1.958800,0.0,0.0,0.0,0.0,0.0 +0,1.958900,0.0,0.0,0.0,0.0,0.0 +0,1.959000,0.0,0.0,0.0,0.0,0.0 +0,1.959100,0.0,0.0,0.0,0.0,0.0 +0,1.959200,0.0,0.0,0.0,0.0,0.0 +0,1.959300,0.0,0.0,0.0,0.0,0.0 +0,1.959400,0.0,0.0,0.0,0.0,0.0 +0,1.959500,0.0,0.0,0.0,0.0,0.0 +0,1.959600,0.0,0.0,0.0,0.0,0.0 +0,1.959700,0.0,0.0,0.0,0.0,0.0 +0,1.959800,0.0,0.0,0.0,0.0,0.0 +0,1.959900,0.0,0.0,0.0,0.0,0.0 +0,1.960000,0.0,0.0,0.0,0.0,0.0 +0,1.960100,0.0,0.0,0.0,0.0,0.0 +1,3138.027020,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.960200,0.0,0.0,0.0,0.0,0.0 +0,1.960300,0.0,0.0,0.0,0.0,0.0 +0,1.960400,0.0,0.0,0.0,0.0,0.0 +0,1.960500,0.0,0.0,0.0,0.0,0.0 +0,1.960600,0.0,0.0,0.0,0.0,0.0 +0,1.960700,0.0,0.0,0.0,0.0,0.0 +0,1.960800,0.0,0.0,0.0,0.0,0.0 +0,1.960900,0.0,0.0,0.0,0.0,0.0 +0,1.961000,0.0,0.0,0.0,0.0,0.0 +0,1.961100,0.0,0.0,0.0,0.0,0.0 +0,1.961200,0.0,0.0,0.0,0.0,0.0 +0,1.961300,0.0,0.0,0.0,0.0,0.0 +0,1.961400,0.0,0.0,0.0,0.0,0.0 +0,1.961500,0.0,0.0,0.0,0.0,0.0 +0,1.961600,0.0,0.0,0.0,0.0,0.0 +0,1.961700,0.0,0.0,0.0,0.0,0.0 +0,1.961800,0.0,0.0,0.0,0.0,0.0 +0,1.961900,0.0,0.0,0.0,0.0,0.0 +0,1.962000,0.0,0.0,0.0,0.0,0.0 +0,1.962100,0.0,0.0,0.0,0.0,0.0 +0,1.962200,0.0,0.0,0.0,0.0,0.0 +0,1.962300,0.0,0.0,0.0,0.0,0.0 +0,1.962400,0.0,0.0,0.0,0.0,0.0 +0,1.962500,0.0,0.0,0.0,0.0,0.0 +0,1.962600,0.0,0.0,0.0,0.0,0.0 +0,1.962700,0.0,0.0,0.0,0.0,0.0 +0,1.962800,0.0,0.0,0.0,0.0,0.0 +0,1.962900,0.0,0.0,0.0,0.0,0.0 +0,1.963000,0.0,0.0,0.0,0.0,0.0 +0,1.963100,0.0,0.0,0.0,0.0,0.0 +0,1.963200,0.0,0.0,0.0,0.0,0.0 +0,1.963300,0.0,0.0,0.0,0.0,0.0 +0,1.963400,0.0,0.0,0.0,0.0,0.0 +0,1.963500,0.0,0.0,0.0,0.0,0.0 +0,1.963600,0.0,0.0,0.0,0.0,0.0 +0,1.963700,0.0,0.0,0.0,0.0,0.0 +0,1.963800,0.0,0.0,0.0,0.0,0.0 +0,1.963900,0.0,0.0,0.0,0.0,0.0 +0,1.964000,0.0,0.0,0.0,0.0,0.0 +0,1.964100,0.0,0.0,0.0,0.0,0.0 +0,1.964200,0.0,0.0,0.0,0.0,0.0 +0,1.964300,0.0,0.0,0.0,0.0,0.0 +0,1.964400,0.0,0.0,0.0,0.0,0.0 +0,1.964500,0.0,0.0,0.0,0.0,0.0 +0,1.964600,0.0,0.0,0.0,0.0,0.0 +0,1.964700,0.0,0.0,0.0,0.0,0.0 +0,1.964800,0.0,0.0,0.0,0.0,0.0 +0,1.964900,0.0,0.0,0.0,0.0,0.0 +0,1.965000,0.0,0.0,0.0,0.0,0.0 +0,1.965100,0.0,0.0,0.0,0.0,0.0 +0,1.965200,0.0,0.0,0.0,0.0,0.0 +0,1.965300,0.0,0.0,0.0,0.0,0.0 +0,1.965400,0.0,0.0,0.0,0.0,0.0 +0,1.965500,0.0,0.0,0.0,0.0,0.0 +0,1.965600,0.0,0.0,0.0,0.0,0.0 +0,1.965700,0.0,0.0,0.0,0.0,0.0 +0,1.965800,0.0,0.0,0.0,0.0,0.0 +0,1.965900,0.0,0.0,0.0,0.0,0.0 +0,1.966000,0.0,0.0,0.0,0.0,0.0 +0,1.966100,0.0,0.0,0.0,0.0,0.0 +0,1.966200,0.0,0.0,0.0,0.0,0.0 +0,1.966300,0.0,0.0,0.0,0.0,0.0 +0,1.966400,0.0,0.0,0.0,0.0,0.0 +0,1.966500,0.0,0.0,0.0,0.0,0.0 +0,1.966600,0.0,0.0,0.0,0.0,0.0 +0,1.966700,0.0,0.0,0.0,0.0,0.0 +0,1.966800,0.0,0.0,0.0,0.0,0.0 +0,1.966900,0.0,0.0,0.0,0.0,0.0 +0,1.967000,0.0,0.0,0.0,0.0,0.0 +0,1.967100,0.0,0.0,0.0,0.0,0.0 +0,1.967200,0.0,0.0,0.0,0.0,0.0 +0,1.967300,0.0,0.0,0.0,0.0,0.0 +0,1.967400,0.0,0.0,0.0,0.0,0.0 +0,1.967500,0.0,0.0,0.0,0.0,0.0 +0,1.967600,0.0,0.0,0.0,0.0,0.0 +0,1.967700,0.0,0.0,0.0,0.0,0.0 +0,1.967800,0.0,0.0,0.0,0.0,0.0 +0,1.967900,0.0,0.0,0.0,0.0,0.0 +0,1.968000,0.0,0.0,0.0,0.0,0.0 +0,1.968100,0.0,0.0,0.0,0.0,0.0 +0,1.968200,0.0,0.0,0.0,0.0,0.0 +0,1.968300,0.0,0.0,0.0,0.0,0.0 +0,1.968400,0.0,0.0,0.0,0.0,0.0 +0,1.968500,0.0,0.0,0.0,0.0,0.0 +0,1.968600,0.0,0.0,0.0,0.0,0.0 +0,1.968700,0.0,0.0,0.0,0.0,0.0 +0,1.968800,0.0,0.0,0.0,0.0,0.0 +0,1.968900,0.0,0.0,0.0,0.0,0.0 +0,1.969000,0.0,0.0,0.0,0.0,0.0 +0,1.969100,0.0,0.0,0.0,0.0,0.0 +0,1.969200,0.0,0.0,0.0,0.0,0.0 +0,1.969300,0.0,0.0,0.0,0.0,0.0 +0,1.969400,0.0,0.0,0.0,0.0,0.0 +0,1.969500,0.0,0.0,0.0,0.0,0.0 +0,1.969600,0.0,0.0,0.0,0.0,0.0 +0,1.969700,0.0,0.0,0.0,0.0,0.0 +0,1.969800,0.0,0.0,0.0,0.0,0.0 +0,1.969900,0.0,0.0,0.0,0.0,0.0 +0,1.970000,0.0,0.0,0.0,0.0,0.0 +0,1.970100,0.0,0.0,0.0,0.0,0.0 +0,1.970200,0.0,0.0,0.0,0.0,0.0 +0,1.970300,0.0,0.0,0.0,0.0,0.0 +0,1.970400,0.0,0.0,0.0,0.0,0.0 +0,1.970500,0.0,0.0,0.0,0.0,0.0 +0,1.970600,0.0,0.0,0.0,0.0,0.0 +0,1.970700,0.0,0.0,0.0,0.0,0.0 +0,1.970800,0.0,0.0,0.0,0.0,0.0 +0,1.970900,0.0,0.0,0.0,0.0,0.0 +0,1.971000,0.0,0.0,0.0,0.0,0.0 +0,1.971100,0.0,0.0,0.0,0.0,0.0 +0,1.971200,0.0,0.0,0.0,0.0,0.0 +0,1.971300,0.0,0.0,0.0,0.0,0.0 +0,1.971400,0.0,0.0,0.0,0.0,0.0 +0,1.971500,0.0,0.0,0.0,0.0,0.0 +0,1.971600,0.0,0.0,0.0,0.0,0.0 +0,1.971700,0.0,0.0,0.0,0.0,0.0 +0,1.971800,0.0,0.0,0.0,0.0,0.0 +0,1.971900,0.0,0.0,0.0,0.0,0.0 +0,1.972000,0.0,0.0,0.0,0.0,0.0 +0,1.972100,0.0,0.0,0.0,0.0,0.0 +0,1.972200,0.0,0.0,0.0,0.0,0.0 +0,1.972300,0.0,0.0,0.0,0.0,0.0 +0,1.972400,0.0,0.0,0.0,0.0,0.0 +0,1.972500,0.0,0.0,0.0,0.0,0.0 +0,1.972600,0.0,0.0,0.0,0.0,0.0 +0,1.972700,0.0,0.0,0.0,0.0,0.0 +0,1.972800,0.0,0.0,0.0,0.0,0.0 +0,1.972900,0.0,0.0,0.0,0.0,0.0 +0,1.973000,0.0,0.0,0.0,0.0,0.0 +0,1.973100,0.0,0.0,0.0,0.0,0.0 +0,1.973200,0.0,0.0,0.0,0.0,0.0 +0,1.973300,0.0,0.0,0.0,0.0,0.0 +0,1.973400,0.0,0.0,0.0,0.0,0.0 +0,1.973500,0.0,0.0,0.0,0.0,0.0 +0,1.973600,0.0,0.0,0.0,0.0,0.0 +0,1.973700,0.0,0.0,0.0,0.0,0.0 +0,1.973800,0.0,0.0,0.0,0.0,0.0 +0,1.973900,0.0,0.0,0.0,0.0,0.0 +0,1.974000,0.0,0.0,0.0,0.0,0.0 +0,1.974100,0.0,0.0,0.0,0.0,0.0 +0,1.974200,0.0,0.0,0.0,0.0,0.0 +0,1.974300,0.0,0.0,0.0,0.0,0.0 +0,1.974400,0.0,0.0,0.0,0.0,0.0 +0,1.974500,0.0,0.0,0.0,0.0,0.0 +0,1.974600,0.0,0.0,0.0,0.0,0.0 +0,1.974700,0.0,0.0,0.0,0.0,0.0 +0,1.974800,0.0,0.0,0.0,0.0,0.0 +0,1.974900,0.0,0.0,0.0,0.0,0.0 +0,1.975000,0.0,0.0,0.0,0.0,0.0 +0,1.975100,0.0,0.0,0.0,0.0,0.0 +0,1.975200,0.0,0.0,0.0,0.0,0.0 +0,1.975300,0.0,0.0,0.0,0.0,0.0 +0,1.975400,0.0,0.0,0.0,0.0,0.0 +0,1.975500,0.0,0.0,0.0,0.0,0.0 +0,1.975600,0.0,0.0,0.0,0.0,0.0 +0,1.975700,0.0,0.0,0.0,0.0,0.0 +0,1.975800,0.0,0.0,0.0,0.0,0.0 +0,1.975900,0.0,0.0,0.0,0.0,0.0 +0,1.976000,0.0,0.0,0.0,0.0,0.0 +0,1.976100,0.0,0.0,0.0,0.0,0.0 +0,1.976200,0.0,0.0,0.0,0.0,0.0 +0,1.976300,0.0,0.0,0.0,0.0,0.0 +0,1.976400,0.0,0.0,0.0,0.0,0.0 +0,1.976500,0.0,0.0,0.0,0.0,0.0 +0,1.976600,0.0,0.0,0.0,0.0,0.0 +0,1.976700,0.0,0.0,0.0,0.0,0.0 +0,1.976800,0.0,0.0,0.0,0.0,0.0 +0,1.976900,0.0,0.0,0.0,0.0,0.0 +0,1.977000,0.0,0.0,0.0,0.0,0.0 +0,1.977100,0.0,0.0,0.0,0.0,0.0 +0,1.977200,0.0,0.0,0.0,0.0,0.0 +0,1.977300,0.0,0.0,0.0,0.0,0.0 +0,1.977400,0.0,0.0,0.0,0.0,0.0 +0,1.977500,0.0,0.0,0.0,0.0,0.0 +0,1.977600,0.0,0.0,0.0,0.0,0.0 +0,1.977700,0.0,0.0,0.0,0.0,0.0 +0,1.977800,0.0,0.0,0.0,0.0,0.0 +0,1.977900,0.0,0.0,0.0,0.0,0.0 +0,1.978000,0.0,0.0,0.0,0.0,0.0 +0,1.978100,0.0,0.0,0.0,0.0,0.0 +0,1.978200,0.0,0.0,0.0,0.0,0.0 +0,1.978300,0.0,0.0,0.0,0.0,0.0 +0,1.978400,0.0,0.0,0.0,0.0,0.0 +0,1.978500,0.0,0.0,0.0,0.0,0.0 +0,1.978600,0.0,0.0,0.0,0.0,0.0 +0,1.978700,0.0,0.0,0.0,0.0,0.0 +0,1.978800,0.0,0.0,0.0,0.0,0.0 +0,1.978900,0.0,0.0,0.0,0.0,0.0 +0,1.979000,0.0,0.0,0.0,0.0,0.0 +0,1.979100,0.0,0.0,0.0,0.0,0.0 +0,1.979200,0.0,0.0,0.0,0.0,0.0 +0,1.979300,0.0,0.0,0.0,0.0,0.0 +0,1.979400,0.0,0.0,0.0,0.0,0.0 +0,1.979500,0.0,0.0,0.0,0.0,0.0 +0,1.979600,0.0,0.0,0.0,0.0,0.0 +0,1.979700,0.0,0.0,0.0,0.0,0.0 +0,1.979800,0.0,0.0,0.0,0.0,0.0 +0,1.979900,0.0,0.0,0.0,0.0,0.0 +0,1.980000,0.0,0.0,0.0,0.0,0.0 +0,1.980100,0.0,0.0,0.0,0.0,0.0 +1,3235.065129,0.0,0.0,0.0,0.0,0.0,1.0 +0,1.980200,0.0,0.0,0.0,0.0,0.0 +0,1.980300,0.0,0.0,0.0,0.0,0.0 +0,1.980400,0.0,0.0,0.0,0.0,0.0 +0,1.980500,0.0,0.0,0.0,0.0,0.0 +0,1.980600,0.0,0.0,0.0,0.0,0.0 +0,1.980700,0.0,0.0,0.0,0.0,0.0 +0,1.980800,0.0,0.0,0.0,0.0,0.0 +0,1.980900,0.0,0.0,0.0,0.0,0.0 +0,1.981000,0.0,0.0,0.0,0.0,0.0 +0,1.981100,0.0,0.0,0.0,0.0,0.0 +0,1.981200,0.0,0.0,0.0,0.0,0.0 +0,1.981300,0.0,0.0,0.0,0.0,0.0 +0,1.981400,0.0,0.0,0.0,0.0,0.0 +0,1.981500,0.0,0.0,0.0,0.0,0.0 +0,1.981600,0.0,0.0,0.0,0.0,0.0 +0,1.981700,0.0,0.0,0.0,0.0,0.0 +0,1.981800,0.0,0.0,0.0,0.0,0.0 +0,1.981900,0.0,0.0,0.0,0.0,0.0 +0,1.982000,0.0,0.0,0.0,0.0,0.0 +0,1.982100,0.0,0.0,0.0,0.0,0.0 +0,1.982200,0.0,0.0,0.0,0.0,0.0 +0,1.982300,0.0,0.0,0.0,0.0,0.0 +0,1.982400,0.0,0.0,0.0,0.0,0.0 +0,1.982500,0.0,0.0,0.0,0.0,0.0 +0,1.982600,0.0,0.0,0.0,0.0,0.0 +0,1.982700,0.0,0.0,0.0,0.0,0.0 +0,1.982800,0.0,0.0,0.0,0.0,0.0 +0,1.982900,0.0,0.0,0.0,0.0,0.0 +0,1.983000,0.0,0.0,0.0,0.0,0.0 +0,1.983100,0.0,0.0,0.0,0.0,0.0 +0,1.983200,0.0,0.0,0.0,0.0,0.0 +0,1.983300,0.0,0.0,0.0,0.0,0.0 +0,1.983400,0.0,0.0,0.0,0.0,0.0 +0,1.983500,0.0,0.0,0.0,0.0,0.0 +0,1.983600,0.0,0.0,0.0,0.0,0.0 +0,1.983700,0.0,0.0,0.0,0.0,0.0 +0,1.983800,0.0,0.0,0.0,0.0,0.0 +0,1.983900,0.0,0.0,0.0,0.0,0.0 +0,1.984000,0.0,0.0,0.0,0.0,0.0 +0,1.984100,0.0,0.0,0.0,0.0,0.0 +0,1.984200,0.0,0.0,0.0,0.0,0.0 +0,1.984300,0.0,0.0,0.0,0.0,0.0 +0,1.984400,0.0,0.0,0.0,0.0,0.0 +0,1.984500,0.0,0.0,0.0,0.0,0.0 +0,1.984600,0.0,0.0,0.0,0.0,0.0 +0,1.984700,0.0,0.0,0.0,0.0,0.0 +0,1.984800,0.0,0.0,0.0,0.0,0.0 +0,1.984900,0.0,0.0,0.0,0.0,0.0 +0,1.985000,0.0,0.0,0.0,0.0,0.0 +0,1.985100,0.0,0.0,0.0,0.0,0.0 +0,1.985200,0.0,0.0,0.0,0.0,0.0 +0,1.985300,0.0,0.0,0.0,0.0,0.0 +0,1.985400,0.0,0.0,0.0,0.0,0.0 +0,1.985500,0.0,0.0,0.0,0.0,0.0 +0,1.985600,0.0,0.0,0.0,0.0,0.0 +0,1.985700,0.0,0.0,0.0,0.0,0.0 +0,1.985800,0.0,0.0,0.0,0.0,0.0 +0,1.985900,0.0,0.0,0.0,0.0,0.0 +0,1.986000,0.0,0.0,0.0,0.0,0.0 +0,1.986100,0.0,0.0,0.0,0.0,0.0 +0,1.986200,0.0,0.0,0.0,0.0,0.0 +0,1.986300,0.0,0.0,0.0,0.0,0.0 +0,1.986400,0.0,0.0,0.0,0.0,0.0 +0,1.986500,0.0,0.0,0.0,0.0,0.0 +0,1.986600,0.0,0.0,0.0,0.0,0.0 +0,1.986700,0.0,0.0,0.0,0.0,0.0 +0,1.986800,0.0,0.0,0.0,0.0,0.0 +0,1.986900,0.0,0.0,0.0,0.0,0.0 +0,1.987000,0.0,0.0,0.0,0.0,0.0 +0,1.987100,0.0,0.0,0.0,0.0,0.0 +0,1.987200,0.0,0.0,0.0,0.0,0.0 +0,1.987300,0.0,0.0,0.0,0.0,0.0 +0,1.987400,0.0,0.0,0.0,0.0,0.0 +0,1.987500,0.0,0.0,0.0,0.0,0.0 +0,1.987600,0.0,0.0,0.0,0.0,0.0 +0,1.987700,0.0,0.0,0.0,0.0,0.0 +0,1.987800,0.0,0.0,0.0,0.0,0.0 +0,1.987900,0.0,0.0,0.0,0.0,0.0 +0,1.988000,0.0,0.0,0.0,0.0,0.0 +0,1.988100,0.0,0.0,0.0,0.0,0.0 +0,1.988200,0.0,0.0,0.0,0.0,0.0 +0,1.988300,0.0,0.0,0.0,0.0,0.0 +0,1.988400,0.0,0.0,0.0,0.0,0.0 +0,1.988500,0.0,0.0,0.0,0.0,0.0 +0,1.988600,0.0,0.0,0.0,0.0,0.0 +0,1.988700,0.0,0.0,0.0,0.0,0.0 +0,1.988800,0.0,0.0,0.0,0.0,0.0 +0,1.988900,0.0,0.0,0.0,0.0,0.0 +0,1.989000,0.0,0.0,0.0,0.0,0.0 +0,1.989100,0.0,0.0,0.0,0.0,0.0 +0,1.989200,0.0,0.0,0.0,0.0,0.0 +0,1.989300,0.0,0.0,0.0,0.0,0.0 +0,1.989400,0.0,0.0,0.0,0.0,0.0 +0,1.989500,0.0,0.0,0.0,0.0,0.0 +0,1.989600,0.0,0.0,0.0,0.0,0.0 +0,1.989700,0.0,0.0,0.0,0.0,0.0 +0,1.989800,0.0,0.0,0.0,0.0,0.0 +0,1.989900,0.0,0.0,0.0,0.0,0.0 +0,1.990000,0.0,0.0,0.0,0.0,0.0 +0,1.990100,0.0,0.0,0.0,0.0,0.0 +0,1.990200,0.0,0.0,0.0,0.0,0.0 +0,1.990300,0.0,0.0,0.0,0.0,0.0 +0,1.990400,0.0,0.0,0.0,0.0,0.0 +0,1.990500,0.0,0.0,0.0,0.0,0.0 +0,1.990600,0.0,0.0,0.0,0.0,0.0 +0,1.990700,0.0,0.0,0.0,0.0,0.0 +0,1.990800,0.0,0.0,0.0,0.0,0.0 +0,1.990900,0.0,0.0,0.0,0.0,0.0 +0,1.991000,0.0,0.0,0.0,0.0,0.0 +0,1.991100,0.0,0.0,0.0,0.0,0.0 +0,1.991200,0.0,0.0,0.0,0.0,0.0 +0,1.991300,0.0,0.0,0.0,0.0,0.0 +0,1.991400,0.0,0.0,0.0,0.0,0.0 +0,1.991500,0.0,0.0,0.0,0.0,0.0 +0,1.991600,0.0,0.0,0.0,0.0,0.0 +0,1.991700,0.0,0.0,0.0,0.0,0.0 +0,1.991800,0.0,0.0,0.0,0.0,0.0 +0,1.991900,0.0,0.0,0.0,0.0,0.0 +0,1.992000,0.0,0.0,0.0,0.0,0.0 +0,1.992100,0.0,0.0,0.0,0.0,0.0 +0,1.992200,0.0,0.0,0.0,0.0,0.0 +0,1.992300,0.0,0.0,0.0,0.0,0.0 +0,1.992400,0.0,0.0,0.0,0.0,0.0 +0,1.992500,0.0,0.0,0.0,0.0,0.0 +0,1.992600,0.0,0.0,0.0,0.0,0.0 +0,1.992700,0.0,0.0,0.0,0.0,0.0 +0,1.992800,0.0,0.0,0.0,0.0,0.0 +0,1.992900,0.0,0.0,0.0,0.0,0.0 +0,1.993000,0.0,0.0,0.0,0.0,0.0 +0,1.993100,0.0,0.0,0.0,0.0,0.0 +0,1.993200,0.0,0.0,0.0,0.0,0.0 +0,1.993300,0.0,0.0,0.0,0.0,0.0 +0,1.993400,0.0,0.0,0.0,0.0,0.0 +0,1.993500,0.0,0.0,0.0,0.0,0.0 +0,1.993600,0.0,0.0,0.0,0.0,0.0 +0,1.993700,0.0,0.0,0.0,0.0,0.0 +0,1.993800,0.0,0.0,0.0,0.0,0.0 +0,1.993900,0.0,0.0,0.0,0.0,0.0 +0,1.994000,0.0,0.0,0.0,0.0,0.0 +0,1.994100,0.0,0.0,0.0,0.0,0.0 +0,1.994200,0.0,0.0,0.0,0.0,0.0 +0,1.994300,0.0,0.0,0.0,0.0,0.0 +0,1.994400,0.0,0.0,0.0,0.0,0.0 +0,1.994500,0.0,0.0,0.0,0.0,0.0 +0,1.994600,0.0,0.0,0.0,0.0,0.0 +0,1.994700,0.0,0.0,0.0,0.0,0.0 +0,1.994800,0.0,0.0,0.0,0.0,0.0 +0,1.994900,0.0,0.0,0.0,0.0,0.0 +0,1.995000,0.0,0.0,0.0,0.0,0.0 +0,1.995100,0.0,0.0,0.0,0.0,0.0 +0,1.995200,0.0,0.0,0.0,0.0,0.0 +0,1.995300,0.0,0.0,0.0,0.0,0.0 +0,1.995400,0.0,0.0,0.0,0.0,0.0 +0,1.995500,0.0,0.0,0.0,0.0,0.0 +0,1.995600,0.0,0.0,0.0,0.0,0.0 +0,1.995700,0.0,0.0,0.0,0.0,0.0 +0,1.995800,0.0,0.0,0.0,0.0,0.0 +0,1.995900,0.0,0.0,0.0,0.0,0.0 +0,1.996000,0.0,0.0,0.0,0.0,0.0 +0,1.996100,0.0,0.0,0.0,0.0,0.0 +0,1.996200,0.0,0.0,0.0,0.0,0.0 +0,1.996300,0.0,0.0,0.0,0.0,0.0 +0,1.996400,0.0,0.0,0.0,0.0,0.0 +0,1.996500,0.0,0.0,0.0,0.0,0.0 +0,1.996600,0.0,0.0,0.0,0.0,0.0 +0,1.996700,0.0,0.0,0.0,0.0,0.0 +0,1.996800,0.0,0.0,0.0,0.0,0.0 +0,1.996900,0.0,0.0,0.0,0.0,0.0 +0,1.997000,0.0,0.0,0.0,0.0,0.0 +0,1.997100,0.0,0.0,0.0,0.0,0.0 +0,1.997200,0.0,0.0,0.0,0.0,0.0 +0,1.997300,0.0,0.0,0.0,0.0,0.0 +0,1.997400,0.0,0.0,0.0,0.0,0.0 +0,1.997500,0.0,0.0,0.0,0.0,0.0 +0,1.997600,0.0,0.0,0.0,0.0,0.0 +0,1.997700,0.0,0.0,0.0,0.0,0.0 +0,1.997800,0.0,0.0,0.0,0.0,0.0 +0,1.997900,0.0,0.0,0.0,0.0,0.0 +0,1.998000,0.0,0.0,0.0,0.0,0.0 +0,1.998100,0.0,0.0,0.0,0.0,0.0 +0,1.998200,0.0,0.0,0.0,0.0,0.0 +0,1.998300,0.0,0.0,0.0,0.0,0.0 +0,1.998400,0.0,0.0,0.0,0.0,0.0 +0,1.998500,0.0,0.0,0.0,0.0,0.0 +0,1.998600,0.0,0.0,0.0,0.0,0.0 +0,1.998700,0.0,0.0,0.0,0.0,0.0 +0,1.998800,0.0,0.0,0.0,0.0,0.0 +0,1.998900,0.0,0.0,0.0,0.0,0.0 +0,1.999000,0.0,0.0,0.0,0.0,0.0 +0,1.999100,0.0,0.0,0.0,0.0,0.0 +0,1.999200,0.0,0.0,0.0,0.0,0.0 +0,1.999300,0.0,0.0,0.0,0.0,0.0 +0,1.999400,0.0,0.0,0.0,0.0,0.0 +0,1.999500,0.0,0.0,0.0,0.0,0.0 +0,1.999600,0.0,0.0,0.0,0.0,0.0 +0,1.999700,0.0,0.0,0.0,0.0,0.0 +0,1.999800,0.0,0.0,0.0,0.0,0.0 +0,1.999900,0.0,0.0,0.0,0.0,0.0 +0,2.000000,0.0,0.0,0.0,0.0,0.0 diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp new file mode 100644 index 000000000..a8e50e1f1 --- /dev/null +++ b/examples/ImuFactorsExample.cpp @@ -0,0 +1,269 @@ +/* ---------------------------------------------------------------------------- + + * 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 imuFactorsExample + * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code. + * @author Garrett (ghemann@gmail.com), Luca Carlone + */ + +/** + * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS + * - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting) + * the line #define USE_COMBINED (few lines below) + * - we read IMU and GPS data from a CSV file, with the following format: + * A row starting with "i" is the first initial position formatted with + * N, E, D, qx, qY, qZ, qW, velN, velE, velD + * A row starting with "0" is an imu measurement + * linAccN, linAccE, linAccD, angVelN, angVelE, angVelD + * A row starting with "1" is a gps correction formatted with + * N, E, D, qX, qY, qZ, qW + * Note that for GPS correction, we're only using the position not the rotation. The + * rotation is provided in the file for ground truth comparison. + */ + +// GTSAM related includes. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor. +// #define USE_COMBINED + +using namespace gtsam; +using namespace std; + +using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) +using symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) + +const string output_filename = "imuFactorExampleResults.csv"; + +// This will either be PreintegratedImuMeasurements (for ImuFactor) or +// PreintegratedCombinedMeasurements (for CombinedImuFactor). +PreintegrationType *imu_preintegrated_; + +int main(int argc, char* argv[]) +{ + string data_filename; + if (argc < 2) { + printf("using default CSV file\n"); + data_filename = findExampleDataFile("imuAndGPSdata.csv"); + } else { + data_filename = argv[1]; + } + + // Set up output file for plotting errors + FILE* fp_out = fopen(output_filename.c_str(), "w+"); + fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n"); + + // Begin parsing the CSV file. Input the first line for initialization. + // From there, we'll iterate through the file and we'll preintegrate the IMU + // or add in the GPS given the input. + ifstream file(data_filename.c_str()); + string value; + + // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD) + Eigen::Matrix initial_state = Eigen::Matrix::Zero(); + getline(file, value, ','); // i + for (int i=0; i<9; i++) { + getline(file, value, ','); + initial_state(i) = atof(value.c_str()); + } + getline(file, value, '\n'); + initial_state(9) = atof(value.c_str()); + cout << "initial state:\n" << initial_state.transpose() << "\n\n"; + + // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z); + Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), + initial_state(4), initial_state(5)); + Point3 prior_point(initial_state.head<3>()); + Pose3 prior_pose(prior_rotation, prior_point); + Vector3 prior_velocity(initial_state.tail<3>()); + imuBias::ConstantBias prior_imu_bias; // assume zero initial bias + + Values initial_values; + int correction_count = 0; + initial_values.insert(X(correction_count), prior_pose); + initial_values.insert(V(correction_count), prior_velocity); + initial_values.insert(B(correction_count), prior_imu_bias); + + // Assemble prior noise model and add it the graph. + noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m + noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s + noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3); + + // Add all prior factors (pose, velocity, bias) to the graph. + NonlinearFactorGraph *graph = new NonlinearFactorGraph(); + graph->add(PriorFactor(X(correction_count), prior_pose, pose_noise_model)); + graph->add(PriorFactor(V(correction_count), prior_velocity,velocity_noise_model)); + graph->add(PriorFactor(B(correction_count), prior_imu_bias,bias_noise_model)); + + // We use the sensor specs to build the noise model for the IMU factor. + double accel_noise_sigma = 0.0003924; + double gyro_noise_sigma = 0.000205689024915; + double accel_bias_rw_sigma = 0.004905; + double gyro_bias_rw_sigma = 0.000001454441043; + Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2); + Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2); + Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities + Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2); + Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2); + Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration + + boost::shared_ptr p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); + // PreintegrationBase params: + p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous + p->integrationCovariance = integration_error_cov; // integration uncertainty continuous + // should be using 2nd order integration + // PreintegratedRotation params: + p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous + // PreintegrationCombinedMeasurements params: + p->biasAccCovariance = bias_acc_cov; // acc bias in continuous + p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous + p->biasAccOmegaInt = bias_acc_omega_int; + +#ifdef USE_COMBINED + imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias); +#else + imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias); +#endif + + // Store previous state for the imu integration and the latest predicted outcome. + NavState prev_state(prior_pose, prior_velocity); + NavState prop_state = prev_state; + imuBias::ConstantBias prev_bias = prior_imu_bias; + + // Keep track of the total error over the entire run for a simple performance metric. + double current_position_error = 0.0, current_orientation_error = 0.0; + + double output_time = 0.0; + double dt = 0.005; // The real system has noise, but here, results are nearly + // exactly the same, so keeping this for simplicity. + + // All priors have been set up, now iterate through the data file. + while (file.good()) { + + // Parse out first value + getline(file, value, ','); + int type = atoi(value.c_str()); + + if (type == 0) { // IMU measurement + Eigen::Matrix imu = Eigen::Matrix::Zero(); + for (int i=0; i<5; ++i) { + getline(file, value, ','); + imu(i) = atof(value.c_str()); + } + getline(file, value, '\n'); + imu(5) = atof(value.c_str()); + + // Adding the IMU preintegration. + imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); + + } else if (type == 1) { // GPS measurement + Eigen::Matrix gps = Eigen::Matrix::Zero(); + for (int i=0; i<6; ++i) { + getline(file, value, ','); + gps(i) = atof(value.c_str()); + } + getline(file, value, '\n'); + gps(6) = atof(value.c_str()); + + correction_count++; + + // Adding IMU factor and GPS factor and optimizing. +#ifdef USE_COMBINED + PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast(imu_preintegrated_); + CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1), + X(correction_count ), V(correction_count ), + B(correction_count-1), B(correction_count ), + *preint_imu_combined); + graph->add(imu_factor); +#else + PreintegratedImuMeasurements *preint_imu = dynamic_cast(imu_preintegrated_); + ImuFactor imu_factor(X(correction_count-1), V(correction_count-1), + X(correction_count ), V(correction_count ), + B(correction_count-1), + *preint_imu); + graph->add(imu_factor); + imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + graph->add(BetweenFactor(B(correction_count-1), + B(correction_count ), + zero_bias, bias_noise_model)); +#endif + + noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0); + GPSFactor gps_factor(X(correction_count), + Point3(gps(0), // N, + gps(1), // E, + gps(2)), // D, + correction_noise); + graph->add(gps_factor); + + // Now optimize and compare results. + prop_state = imu_preintegrated_->predict(prev_state, prev_bias); + initial_values.insert(X(correction_count), prop_state.pose()); + initial_values.insert(V(correction_count), prop_state.v()); + initial_values.insert(B(correction_count), prev_bias); + + LevenbergMarquardtOptimizer optimizer(*graph, initial_values); + Values result = optimizer.optimize(); + + // Overwrite the beginning of the preintegration for the next step. + prev_state = NavState(result.at(X(correction_count)), + result.at(V(correction_count))); + prev_bias = result.at(B(correction_count)); + + // Reset the preintegration object. + imu_preintegrated_->resetIntegrationAndSetBias(prev_bias); + + // Print out the position and orientation error for comparison. + Vector3 gtsam_position = prev_state.pose().translation(); + Vector3 position_error = gtsam_position - gps.head<3>(); + current_position_error = position_error.norm(); + + Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion(); + Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5)); + Quaternion quat_error = gtsam_quat * gps_quat.inverse(); + quat_error.normalize(); + Vector3 euler_angle_error(quat_error.x()*2, + quat_error.y()*2, + quat_error.z()*2); + current_orientation_error = euler_angle_error.norm(); + + // display statistics + cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n"; + + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2), + gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(), + gps(0), gps(1), gps(2), + gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w()); + + output_time += 1.0; + + } else { + cerr << "ERROR parsing file\n"; + return 1; + } + } + fclose(fp_out); + cout << "Complete, results written to " << output_filename << "\n\n";; + return 0; +} diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 84bca65f3..17f921fd1 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -120,15 +120,15 @@ int main(int argc, char** argv) { // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); + graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. noiseModel::Diagonal::shared_ptr 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); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index eb6bdd49d..3416eb6b7 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -37,12 +37,12 @@ int main(int argc, char** argv) { Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + graph.emplace_shared >(1, priorMean, priorNoise); Pose2 odometry(2.0, 0.0, 0.0); noiseModel::Diagonal::shared_ptr 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)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print Values initial; diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index b5cd681e5..3b547e70c 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -65,15 +65,15 @@ int main(int argc, char** argv) { // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + graph.emplace_shared >(1, priorMean, priorNoise); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index a716f9cd8..7c43c22e2 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -81,13 +81,13 @@ int main(int argc, char** argv) { // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph + graph.emplace_shared >(x1, prior, priorNoise); // add directly to graph // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(x2, x3, odometry, odometryNoise); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements @@ -101,9 +101,9 @@ int main(int argc, char** argv) { range32 = 2.0; // Add Bearing-Range factors - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 7991f7fbf..f977a08af 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -72,23 +72,23 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint // This factor encodes the fact that we have returned to the same pose. In real systems, // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. We will use another Between Factor to enforce this constraint: - graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 314ccbdb4..99711da2d 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -33,19 +33,19 @@ int main (int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); // 3. Create the data structure to hold the initial estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 2c25d2f8e..9b459d7b8 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -36,18 +36,18 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.push_back(PriorFactor(1, prior, priorNoise)); + graph.emplace_shared >(1, prior, priorNoise); // 2b. Add odometry factors noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add the loop closure constraint noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), model); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index f3f770750..9cab73822 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) { std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl; // Additional: rewrite input with simplified keys 0,1,... Values simpleInitial; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + for(const Values::ConstKeyValuePair& key_value: *initial) { Key key; if(add) key = key_value.key + firstKey; @@ -66,7 +66,7 @@ int main(const int argc, const char *argv[]) { simpleInitial.insert(key, initial->at(key_value.key)); } NonlinearFactorGraph simpleGraph; - BOOST_FOREACH(const boost::shared_ptr& factor, *graph) { + for(const boost::shared_ptr& factor: *graph) { boost::shared_ptr > pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index c0399c139..5066222e5 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) { noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index 645b24cfc..e90d014c0 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) { noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 99342099a..10960cf31 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) { noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); diff --git a/examples/README b/examples/README deleted file mode 100644 index 2a54459f3..000000000 --- a/examples/README +++ /dev/null @@ -1,37 +0,0 @@ -This directory contains a number of examples that illustrate the use of GTSAM: - -SimpleRotation: a super-simple example of optimizing a single rotation according to a single prior - -Kalman Filter Examples -====================== -elaboratePoint2KalmanFilter: simple linear Kalman filter on a moving 2D point, but done using factor graphs -easyPoint2KalmanFilter: uses the cool generic templated Kalman filter class to do the same -fullStateKalmanFilter: simple 1D example with a full-state filter -errorStateKalmanFilter: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias - -2D Pose SLAM -============ -LocalizationExample.cpp: modeling robot motion -LocalizationExample2.cpp: example with GPS like measurements -Pose2SLAMExample: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h -Pose2SLAMExample_advanced: same, but uses an Optimizer object -Pose2SLAMwSPCG: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface - -Planar SLAM with landmarks -========================== -PlanarSLAMExample: simple robotics example using the pre-built planar SLAM domain -PlanarSLAMExample_selfcontained: simple robotics example with all typedefs internal to this script. - -Visual SLAM -=========== -CameraResectioning.cpp: An example of gtsam for solving the camera resectioning problem -The directory vSLAMexample includes 2 simple examples using GTSAM: -- vSFMexample using visualSLAM in for structure-from-motion (SFM), and -- vISAMexample using visualSLAM and ISAM for incremental SLAM updates -See the separate README file there. - -Undirected Graphical Models (UGM) -================================= -The best representation for a Markov Random Field is a factor graph :-) -This is illustrated with some discrete examples from the UGM MATLAB toolbox, which -can be found at http://www.di.ens.fr/~mschmidt/Software/UGM \ No newline at end of file diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 000000000..9d58b5200 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,96 @@ +# GTSAM Examples + +This directory contains all GTSAM C++ examples GTSAM pertaining to SFM + + +## Basic Examples: + +* **SimpleRotation**: a simple example of optimizing a single rotation according to a single prior +* **CameraResectioning**: resection camera from some known points +* **SFMExample**: basic structure from motion +* **SFMExample_bal**: same, but read data from read from BAL file +* **SelfCalibrationExample**: Do SFM while also optimizing for calibration + +## Stereo Visual Odometry Examples +Visual odometry using a stereo rig: + +* **StereoVOExample**: basic example of stereo VO +* **StereoVOExample_large**: larger, with a snippet of Kitti data + +## More Advanced Examples +The following examples illustrate some concepts from Georgia Tech's research papers, listed in the references section at the end: + +* **VisualISAMExample**: uses iSAM [TRO08] +* **VisualISAM2Example**: uses iSAM2 [IJRR12] +* **SFMExample_SmartFactor**: uses smartFactors [ICRA14] + +## Kalman Filter Examples +* **elaboratePoint2KalmanFilter**: simple linear Kalman filter on a moving 2D point, but done using factor graphs +* **easyPoint2KalmanFilter**: uses the generic templated Kalman filter class to do the same +* **fullStateKalmanFilter**: simple 1D example with a full-state filter +* **errorStateKalmanFilter**: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias + +## 2D Pose SLAM + +* **LocalizationExample.cpp**: modeling robot motion +* **LocalizationExample2.cpp**: example with GPS like measurements +* **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h +* **Pose2SLAMExample_advanced**: same, but uses an Optimizer object +* **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface + +## Planar SLAM with landmarks +* **PlanarSLAMExample**: simple robotics example using the pre-built planar SLAM domain +* **PlanarSLAMExample_selfcontained**: simple robotics example with all typedefs internal to this script. + +## Visual SLAM + +The directory **vSLAMexample** includes 2 simple examples using GTSAM: + +- **vSFMexample** using visual SLAM for structure-from-motion (SFM) +- **vISAMexample** using visual SLAM and ISAM for incremental SLAM updates + +See the separate README file there. + +##Undirected Graphical Models (UGM) +The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which +can be found at + + +##Building and Running +To build, cd into the directory and do: + +``` +mkdir build +cd build +cmake .. +``` + +For each .cpp file in this directory two make targets are created, one to build the executable, and one to build and run it. For example, the file `CameraResectioning.cpp` contains simple example to resection a camera from 4 known points. You can build it using + +``` +make CameraResectioning +``` +or build and run it immediately with + +``` +make CameraResectioning.run +``` +which should output: + +``` +Final result: +Values with 1 values: +Value x1: R: +[ + 1, 0.0, 0.0, + 0.0, -1, 0.0, + 0.0, 0.0, -1, +]; +t: [0, 0, 2]'; +``` + + +## References +- [TRO08]: [iSAM: Incremental Smoothing and Mapping, Michael Kaess](http://frank.dellaert.com/pub/Kaess08tro.pdf), Michael Kaess, Ananth Ranganathan, and Frank Dellaert, IEEE Transactions on Robotics, 2008 +- [IJRR12]: [iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree](http://www.cc.gatech.edu/~dellaert/pub/Kaess12ijrr.pdf), Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, and Frank Dellaert, International Journal of Robotics Research, 2012 +- [ICRA14]: [Eliminating Conditionally Independent Sets in Factor Graphs: A Unifying Perspective based on Smart Factors](http://frank.dellaert.com/pub/Carlone14icra.pdf), Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, IEEE International Conference on Robotics and Automation (ICRA), 2014 diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 01ce8b08b..032d61a4a 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -43,7 +43,6 @@ #include // Standard headers, added last, so we know headers above work on their own -#include #include #include @@ -151,7 +150,7 @@ int main (int argc, char** argv) { // Loop over odometry gttic_(iSAM); - BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) { + for(const TimedOdometry& timedOdometry: odometry) { //--------------------------------- odometry loop ----------------------------------------- double t; Pose2 odometry; @@ -196,7 +195,7 @@ int main (int argc, char** argv) { } i += 1; //--------------------------------- odometry loop ----------------------------------------- - } // BOOST_FOREACH + } // end for gttoc_(iSAM); // Print timings diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 893454936..0e48bb892 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -75,14 +75,14 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. This indirectly specifies where the origin is. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph // Simulated measurements from each camera pose, adding them to the factor graph for (size_t i = 0; i < poses.size(); ++i) { SimpleCamera camera(poses[i], *K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } } @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) { // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance // between the first camera and the first landmark. All other landmark positions are interpreted using this scale. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph graph.print("Factor Graph:\n"); // Create the data structure to hold the initial estimate to the solution diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 66beefb35..ea50f7163 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -78,10 +78,10 @@ int main(int argc, char* argv[]) { // Simulated measurements from each camera pose, adding them to the factor graph size_t j = 0; - BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + for(const SfM_Track& track: mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera @@ -98,9 +98,9 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0; j = 0; - BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) + for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera); - BOOST_FOREACH(const SfM_Track& track, mydata.tracks) + for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p); /* Optimize the graph and print results */ diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index e7c0aa696..e9d02b15c 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -82,13 +82,13 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); + graph.emplace_shared >(0, poses[0], noise); // Because the structure-from-motion problem has a scale ambiguity, the problem is // still under-constrained. Here we add a prior on the second pose x1, so this will // fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest of the poses will be also be fixed. - graph.push_back(PriorFactor(1, Camera(poses[1],K), noise)); // add directly to graph + graph.emplace_shared >(1, poses[1], noise); // add directly to graph graph.print("Factor Graph:\n"); @@ -97,7 +97,7 @@ int main(int argc, char* argv[]) { Values initialEstimate; Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(i, Camera(poses[i].compose(delta), K)); + initialEstimate.insert(i, poses[i].compose(delta)); initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 743934c7c..da7c5ba9b 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -74,16 +74,16 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); + graph.emplace_shared >(0, poses[0], noise); // Fix the scale ambiguity by adding a prior - graph.push_back(PriorFactor(1, Camera(poses[0],K), noise)); + graph.emplace_shared >(1, poses[0], noise); // Create the initial estimate to the solution Values initialEstimate; Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(i, Camera(poses[i].compose(delta),K)); + initialEstimate.insert(i, poses[i].compose(delta)); // We will use LM in the outer optimization loop, but by specifying "Iterative" below // We indicate that an iterative linear solver should be used. diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 0e11adaed..a3a416eb3 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -55,25 +55,25 @@ int main (int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; - BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { - BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + for(const SfM_Track& track: mydata.tracks) { + for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; size_t i = 0; j = 0; - BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); - BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); + for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera); + for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p); /* Optimize the graph and print results */ Values result; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 4bbaac3ef..13a07dda5 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -60,25 +60,25 @@ int main (int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; - BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { - BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + for(const SfM_Track& track: mydata.tracks) { + for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; size_t i = 0; j = 0; - BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); - BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); + for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera); + for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p); /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 1c5d155dc..93e010543 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Simulated measurements from each camera pose, adding them to the factor graph Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); @@ -65,17 +65,17 @@ int main(int argc, char* argv[]) { Point2 measurement = camera.project(points[j]); // The only real difference with the Visual SLAM example is that here we use a // different factor type, that also calculates the Jacobian with respect to calibration - graph.push_back(GeneralSFMFactor2(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0))); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)); } } // Add a prior on the position of the first landmark. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph // Add a prior on the calibration. noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished()); - graph.push_back(PriorFactor(Symbol('K', 0), K, calNoise)); + graph.emplace_shared >(Symbol('K', 0), K, calNoise); // Create the initial estimate to the solution // now including an estimate on the camera calibration parameters diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 2000b348b..deaf3b781 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -80,7 +81,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c // the factor graph already includes a factor for the prior/equality constraint. // double dof = graph.size() - config.size(); int graph_dim = 0; - BOOST_FOREACH(const boost::shared_ptr& nlf, graph) { + for(const boost::shared_ptr& nlf: graph) { graph_dim += (int)nlf->dim(); } double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim @@ -421,9 +422,9 @@ void runIncremental() //try { // Marginals marginals(graph, values); // int i=0; - // BOOST_REVERSE_FOREACH(Key key1, values.keys()) { + // for (Key key1: boost::adaptors::reverse(values.keys())) { // int j=0; - // BOOST_REVERSE_FOREACH(Key key2, values.keys()) { + // for (Key key12: boost::adaptors::reverse(values.keys())) { // if(i != j) { // gttic_(jointMarginalInformation); // std::vector keys(2); @@ -442,7 +443,7 @@ void runIncremental() // break; // } // tictoc_print_(); - // BOOST_FOREACH(Key key, values.keys()) { + // for(Key key: values.keys()) { // gttic_(marginalInformation); // Matrix info = marginals.marginalInformation(key); // gttoc_(marginalInformation); @@ -535,7 +536,7 @@ void runCompare() vector commonKeys; br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys)); double maxDiff = 0.0; - BOOST_FOREACH(Key j, commonKeys) + for(Key j: commonKeys) maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm()); cout << " Maximum solution difference (norm of logmap): " << maxDiff << endl; } @@ -549,7 +550,7 @@ void runPerturb() // Perturb values VectorValues noise; - BOOST_FOREACH(const Values::KeyValuePair& key_val, initial) + for(const Values::KeyValuePair& key_val: initial) { Vector noisev(key_val.value.dim()); for(Vector::Index i = 0; i < noisev.size(); ++i) diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 19725798c..27c10deb3 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv){ //create graph object, add first pose at origin with key '1' NonlinearFactorGraph graph; Pose3 first_pose; - graph.push_back(NonlinearEquality(1, Pose3())); + graph.emplace_shared >(1, Pose3()); //create factor noise model with 3 sigmas of value 1 const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); @@ -47,14 +47,14 @@ int main(int argc, char** argv){ const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); //create and add stereo factors between first pose (key value 1) and the three landmarks - graph.push_back(GenericStereoFactor(StereoPoint2(520, 480, 440), model, 1, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(120, 80, 440), model, 1, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 280, 140), model, 1, 5, K)); + graph.emplace_shared >(StereoPoint2(520, 480, 440), model, 1, 3, K); + graph.emplace_shared >(StereoPoint2(120, 80, 440), model, 1, 4, K); + graph.emplace_shared >(StereoPoint2(320, 280, 140), model, 1, 5, K); //create and add stereo factors between second pose and the three landmarks - graph.push_back(GenericStereoFactor(StereoPoint2(570, 520, 490), model, 2, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(70, 20, 490), model, 2, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 270, 115), model, 2, 5, K)); + graph.emplace_shared >(StereoPoint2(570, 520, 490), model, 2, 3, K); + graph.emplace_shared >(StereoPoint2(70, 20, 490), model, 2, 4, K); + graph.emplace_shared >(StereoPoint2(320, 270, 115), model, 2, 5, K); //create Values object to contain initial estimates of camera poses and landmark locations Values initial_estimate; @@ -67,7 +67,7 @@ int main(int argc, char** argv){ initial_estimate.insert(5, Point3(0, -0.5, 5)); //create Levenberg-Marquardt optimizer for resulting factor graph, optimize - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + LevenbergMarquardtOptimizer optimizer(graph, initial_estimate); Values result = optimizer.optimize(); result.print("Final result:\n"); diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index e357745be..80831bd21 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -83,9 +83,9 @@ int main(int argc, char** argv){ cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { - graph.push_back( - GenericStereoFactor(StereoPoint2(uL, uR, v), model, - Symbol('x', x), Symbol('l', l), K)); + graph.emplace_shared< + GenericStereoFactor >(StereoPoint2(uL, uR, v), model, + Symbol('x', x), Symbol('l', l), K); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it if (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); @@ -99,13 +99,13 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph LevenbergMarquardtParams params; params.orderingType = Ordering::METIS; - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); + LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params); Values result = optimizer.optimize(); cout << "Final result sample:" << endl; diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index 602a00593..178fa513f 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -19,8 +19,8 @@ #include #include -#include #include +#include using namespace std; using namespace gtsam; @@ -76,7 +76,7 @@ map testWithoutMemoryAllocation() const vector grainSizes = list_of(1)(10)(100)(1000); map timingResults; - BOOST_FOREACH(size_t grainSize, grainSizes) + for(size_t grainSize: grainSizes) { tbb::tick_count t0 = tbb::tick_count::now(); tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithoutAllocation(results)); @@ -129,7 +129,7 @@ map testWithMemoryAllocation() const vector grainSizes = list_of(1)(10)(100)(1000); map timingResults; - BOOST_FOREACH(size_t grainSize, grainSizes) + for(size_t grainSize: grainSizes) { tbb::tick_count t0 = tbb::tick_count::now(); tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithAllocation(results)); @@ -150,7 +150,7 @@ int main(int argc, char* argv[]) const vector numThreads = list_of(1)(4)(8); Results results; - BOOST_FOREACH(size_t n, numThreads) + for(size_t n: numThreads) { cout << "With " << n << " threads:" << endl; tbb::task_scheduler_init init((int)n); @@ -160,19 +160,19 @@ int main(int argc, char* argv[]) } cout << "Summary of results:" << endl; - BOOST_FOREACH(const Results::value_type& threads_result, results) + for(const Results::value_type& threads_result: results) { const int threads = threads_result.first; const ResultWithThreads& result = threads_result.second; if(threads != 1) { - BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithoutAllocation) + for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithoutAllocation) { const int grainsize = grainsize_time.first; const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second; cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl; } - BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithAllocation) + for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithAllocation) { const int grainsize = grainsize_time.first; const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second; diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 78bc463ec..157768be7 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } // Add an initial guess for the current pose @@ -104,11 +104,11 @@ int main(int argc, char* argv[]) { if( i == 0) { // Add a prior on pose x0 noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index a839289c2..8ca1be596 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -89,9 +89,8 @@ int main(int argc, char* argv[]) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); // Add measurement - graph.add( - GenericProjectionFactor(measurement, noise, - Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, noise, + Symbol('x', i), Symbol('l', j), K); } // Intentionally initialize the variables off from the ground truth @@ -109,12 +108,12 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // Add initial guesses to all observed landmarks Point3 noise(-0.25, 0.20, 0.15); diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 201ec188b..d68cedb74 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -70,7 +70,7 @@ int main() { // Predict the new value with the EKF class Point2 x1_predict = ekf.predict(factor1); - x1_predict.print("X1 Predict"); + traits::Print(x1_predict, "X1 Predict"); @@ -91,7 +91,7 @@ int main() { // Update the Kalman Filter with the measurement Point2 x1_update = ekf.update(factor2); - x1_update.print("X1 Update"); + traits::Print(x1_update, "X1 Update"); @@ -101,13 +101,13 @@ int main() { difference = Point2(1,0); BetweenFactor factor3(x1, x2, difference, Q); Point2 x2_predict = ekf.predict(factor1); - x2_predict.print("X2 Predict"); + traits::Print(x2_predict, "X2 Predict"); // Update Point2 z2(2.0, 0.0); PriorFactor factor4(x2, z2, R); Point2 x2_update = ekf.update(factor4); - x2_update.print("X2 Update"); + traits::Print(x2_update, "X2 Update"); @@ -117,13 +117,13 @@ int main() { difference = Point2(1,0); BetweenFactor factor5(x2, x3, difference, Q); Point2 x3_predict = ekf.predict(factor5); - x3_predict.print("X3 Predict"); + traits::Print(x3_predict, "X3 Predict"); // Update Point2 z3(3.0, 0.0); PriorFactor factor6(x3, z3, R); Point2 x3_update = ekf.update(factor6); - x3_update.print("X3 Update"); + traits::Print(x3_update, "X3 Update"); return 0; } diff --git a/gtsam.h b/gtsam.h index c12055916..7c5bc99b1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -266,23 +266,12 @@ class Point2 { // Group static gtsam::Point2 identity(); - gtsam::Point2 inverse() const; - gtsam::Point2 compose(const gtsam::Point2& p2) const; - gtsam::Point2 between(const gtsam::Point2& p2) const; - - // Manifold - gtsam::Point2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point2& p) const; - - // Lie Group - static gtsam::Point2 Expmap(Vector v); - static Vector Logmap(const gtsam::Point2& p); // Standard Interface double x() const; double y() const; Vector vector() const; - double dist(const gtsam::Point2& p2) const; + double distance(const gtsam::Point2& p2) const; double norm() const; // enabling serialization functionality @@ -1368,7 +1357,7 @@ virtual class HessianFactor : gtsam::GaussianFactor { //Standard Interface size_t rows() const; - Matrix info() const; + Matrix information() const; double constantTerm() const; Vector linearTerm() const; @@ -1941,10 +1930,10 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { gtsam::JacobianFactor* toJacobian() const; gtsam::HessianFactor* toHessian() const; - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, const gtsam::Values& linearizationPoint); - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); // enabling serialization functionality void serializable() const; @@ -2506,30 +2495,8 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { bool getUse2ndOrderCoriolis() const; }; -#include -virtual class PreintegrationBase { - // Constructors - PreintegrationBase(const gtsam::PreintegrationParams* params); - PreintegrationBase(const gtsam::PreintegrationParams* params, - const gtsam::imuBias::ConstantBias& bias); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationBase& expected, double tol); - - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - - // Standard Interface - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - #include -virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { +class PreintegratedImuMeasurements { // Constructors PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, @@ -2544,6 +2511,13 @@ virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { double deltaT); void resetIntegration(); Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; }; virtual class ImuFactor: gtsam::NonlinearFactor { @@ -2559,7 +2533,7 @@ virtual class ImuFactor: gtsam::NonlinearFactor { }; #include -virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { +class PreintegratedCombinedMeasurements { // Testable void print(string s) const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, @@ -2570,6 +2544,13 @@ virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { double deltaT); void resetIntegration(); Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; }; virtual class CombinedImuFactor: gtsam::NonlinearFactor { diff --git a/gtsam/3rdparty/CCOLAMD/Demo/Makefile b/gtsam/3rdparty/CCOLAMD/Demo/Makefile new file mode 100644 index 000000000..c0ad95683 --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Demo/Makefile @@ -0,0 +1,49 @@ +#----------------------------------------------------------------------------- +# compile the CCOLAMD demo +#----------------------------------------------------------------------------- + +default: all + +include ../../SuiteSparse_config/SuiteSparse_config.mk + +I = -I../../include + +C = $(CC) $(CF) $(I) + +LIB2 = $(LDFLAGS) -L../../lib -lccolamd -lsuitesparseconfig $(LDLIBS) + +all: library ccolamd_example ccolamd_l_example + +library: + ( cd ../../SuiteSparse_config ; $(MAKE) ) + ( cd ../Lib ; $(MAKE) ) + +#------------------------------------------------------------------------------ +# Create the demo program, run it, and compare the output +#------------------------------------------------------------------------------ + +dist: + +ccolamd_example: ccolamd_example.c + $(C) -o ccolamd_example ccolamd_example.c $(LIB2) + - ./ccolamd_example > my_ccolamd_example.out + - diff ccolamd_example.out my_ccolamd_example.out + +ccolamd_l_example: ccolamd_l_example.c + $(C) -o ccolamd_l_example ccolamd_l_example.c $(LIB2) + - ./ccolamd_l_example > my_ccolamd_l_example.out + - diff ccolamd_l_example.out my_ccolamd_l_example.out + +#------------------------------------------------------------------------------ +# Remove all but the files in the original distribution +#------------------------------------------------------------------------------ + +clean: + - $(RM) -r $(CLEAN) + +purge: distclean + +distclean: clean + - $(RM) ccolamd_example ccolamd_l_example + - $(RM) my_ccolamd_example.out my_ccolamd_l_example.out + - $(RM) -r $(PURGE) diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example deleted file mode 100755 index aa64a157d..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c index 253fdc988..44423574e 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out index dd2dc4955..ca0dc0021 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out @@ -15,7 +15,7 @@ Column 3, with 2 entries: row 1 row 3 -ccolamd version 2.7, Jan 25, 2011: OK. +ccolamd version 2.9, May 4, 2016: OK. ccolamd: number of dense or empty rows ignored: 0 ccolamd: number of dense or empty columns ignored: 0 ccolamd: number of garbage collections performed: 0 @@ -38,7 +38,7 @@ Column 3, with 1 entries: row 4 Column 4, with 0 entries: -csymamd version 2.7, Jan 25, 2011: OK. +csymamd version 2.9, May 4, 2016: OK. csymamd: number of dense or empty rows ignored: 0 csymamd: number of dense or empty columns ignored: 0 csymamd: number of garbage collections performed: 0 diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example deleted file mode 100755 index a695d3823..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c index b213aad0e..b68e1d0d6 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c @@ -1,12 +1,10 @@ /* ========================================================================== */ -/* === ccolamd and csymamd example (UF_long integer version) ================ */ +/* === ccolamd and csymamd example (long integer version) =================== */ /* ========================================================================== */ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -46,9 +44,6 @@ #define B_NNZ 4 #define B_N 5 -/* define UF_long */ -#include "UFconfig.h" - int main (void) { @@ -56,14 +51,14 @@ int main (void) /* input matrix A definition */ /* ====================================================================== */ - UF_long A [ALEN] = { + SuiteSparse_long A [ALEN] = { 0, 1, 4, /* row indices of nonzeros in column 0 */ 2, 4, /* row indices of nonzeros in column 1 */ 0, 1, 2, 3, /* row indices of nonzeros in column 2 */ 1, 3} ; /* row indices of nonzeros in column 3 */ - UF_long p [ ] = { + SuiteSparse_long p [ ] = { 0, /* column 0 is in A [0..2] */ 3, /* column 1 is in A [3..4] */ @@ -75,7 +70,7 @@ int main (void) /* input matrix B definition */ /* ====================================================================== */ - UF_long B [ ] = { /* Note: only strictly lower triangular part */ + SuiteSparse_long B [ ] = { /* Note: only strictly lower triangular part */ /* is included, since symamd ignores the */ /* diagonal and upper triangular part of B. */ @@ -85,7 +80,7 @@ int main (void) 4 /* row indices of nonzeros in column 3 */ } ; /* row indices of nonzeros in column 4 (none) */ - UF_long q [ ] = { + SuiteSparse_long q [ ] = { 0, /* column 0 is in B [0] */ 1, /* column 1 is in B [1..2] */ @@ -98,10 +93,9 @@ int main (void) /* other variable definitions */ /* ====================================================================== */ - UF_long perm [B_N+1] ; /* note the size is N+1 */ - UF_long stats [CCOLAMD_STATS] ; /* for ccolamd and csymamd output stats */ - - UF_long row, col, pp, length, ok ; + SuiteSparse_long perm [B_N+1] ; /* note the size is N+1 */ + SuiteSparse_long stats [CCOLAMD_STATS] ; /* ccolamd/csymamd output stats */ + SuiteSparse_long row, col, pp, length, ok ; /* ====================================================================== */ /* dump the input matrix A */ diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out index fc87f5474..61bbe1c60 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out @@ -15,7 +15,7 @@ Column 3, with 2 entries: row 1 row 3 -ccolamd version 2.7, Jan 25, 2011: OK. +ccolamd version 2.9, May 4, 2016: OK. ccolamd: number of dense or empty rows ignored: 0 ccolamd: number of dense or empty columns ignored: 0 ccolamd: number of garbage collections performed: 0 @@ -38,7 +38,7 @@ Column 3, with 1 entries: row 4 Column 4, with 0 entries: -csymamd version 2.7, Jan 25, 2011: OK. +csymamd version 2.9, May 4, 2016: OK. csymamd: number of dense or empty rows ignored: 0 csymamd: number of dense or empty columns ignored: 0 csymamd: number of garbage collections performed: 0 diff --git a/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out b/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out deleted file mode 100644 index dd2dc4955..000000000 --- a/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out +++ /dev/null @@ -1,50 +0,0 @@ -ccolamd 5-by-4 input matrix: -Column 0, with 3 entries: - row 0 - row 1 - row 4 -Column 1, with 2 entries: - row 2 - row 4 -Column 2, with 4 entries: - row 0 - row 1 - row 2 - row 3 -Column 3, with 2 entries: - row 1 - row 3 - -ccolamd version 2.7, Jan 25, 2011: OK. -ccolamd: number of dense or empty rows ignored: 0 -ccolamd: number of dense or empty columns ignored: 0 -ccolamd: number of garbage collections performed: 0 -ccolamd column ordering: -1st column: 1 -2nd column: 0 -3rd column: 3 -4th column: 2 - - -csymamd 5-by-5 input matrix: -Entries in strictly lower triangular part: -Column 0, with 1 entries: - row 1 -Column 1, with 2 entries: - row 2 - row 3 -Column 2, with 0 entries: -Column 3, with 1 entries: - row 4 -Column 4, with 0 entries: - -csymamd version 2.7, Jan 25, 2011: OK. -csymamd: number of dense or empty rows ignored: 0 -csymamd: number of dense or empty columns ignored: 0 -csymamd: number of garbage collections performed: 0 -csymamd column ordering: -1st row/column: 0 -2nd row/column: 2 -3rd row/column: 1 -4th row/column: 3 -5th row/column: 4 diff --git a/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out b/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out deleted file mode 100644 index fc87f5474..000000000 --- a/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out +++ /dev/null @@ -1,50 +0,0 @@ -ccolamd 5-by-4 input matrix: -Column 0, with 3 entries: - row 0 - row 1 - row 4 -Column 1, with 2 entries: - row 2 - row 4 -Column 2, with 4 entries: - row 0 - row 1 - row 2 - row 3 -Column 3, with 2 entries: - row 1 - row 3 - -ccolamd version 2.7, Jan 25, 2011: OK. -ccolamd: number of dense or empty rows ignored: 0 -ccolamd: number of dense or empty columns ignored: 0 -ccolamd: number of garbage collections performed: 0 -ccolamd_l column ordering: -1st column: 1 -2nd column: 0 -3rd column: 3 -4th column: 2 - - -csymamd_l 5-by-5 input matrix: -Entries in strictly lower triangular part: -Column 0, with 1 entries: - row 1 -Column 1, with 2 entries: - row 2 - row 3 -Column 2, with 0 entries: -Column 3, with 1 entries: - row 4 -Column 4, with 0 entries: - -csymamd version 2.7, Jan 25, 2011: OK. -csymamd: number of dense or empty rows ignored: 0 -csymamd: number of dense or empty columns ignored: 0 -csymamd: number of garbage collections performed: 0 -csymamd_l column ordering: -1st row/column: 0 -2nd row/column: 2 -3rd row/column: 1 -4th row/column: 3 -5th row/column: 4 diff --git a/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog b/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog index 499d2fc69..0e3eab497 100644 --- a/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog +++ b/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog @@ -1,3 +1,41 @@ +May 4, 2016: version 2.9.6 + + * minor changes to Makefile + +Apr 1, 2016: version 2.9.5 + + * licensing simplified (no other change); refer to CCOLAMD/Doc/License.txt + +Feb 1, 2016: version 2.9.4 + + * update to Makefiles + +Jan 30, 2016: version 2.9.3 + + * modifications to Makefiles + +Jan 1, 2016: version 2.9.2 + + * modified Makefile to create shared libraries + No change to C code except version number. + The empty file ccolamd_global.c removed. + +Oct 10, 2014: version 2.9.1 + + modified MATLAB/ccolamd_make.m. No change to C code except version number. + +July 31, 2013: version 2.9.0 + + * changed malloc and printf pointers to use SuiteSparse_config + +Jun 1, 2012: version 2.8.0 + + * changed from UFconfig to SuiteSparse_config + +Dec 7, 2011: version 2.7.4 + + * fixed the Makefile to better align with CFLAGS and other standards + Jan 25, 2011: version 2.7.3 * minor fix to "make install" diff --git a/gtsam/3rdparty/CCOLAMD/Doc/License.txt b/gtsam/3rdparty/CCOLAMD/Doc/License.txt new file mode 100644 index 000000000..66bb848dc --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Doc/License.txt @@ -0,0 +1,33 @@ +CCOLAMD: constrained column approximate minimum degree ordering +Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis, +Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by +Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. +http://www.suitesparse.com + +-------------------------------------------------------------------------------- + +CCOLAMD license: BSD 3-clause: + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the organizations to which the authors are + affiliated, nor the names of its contributors may be used to endorse + or promote products derived from this software without specific prior + written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY + DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + DAMAGE. diff --git a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h index 55693d47d..8c2129ef3 100644 --- a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h +++ b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -32,24 +30,24 @@ extern "C" { /* All versions of CCOLAMD will include the following definitions. * As an example, to test if the version you are using is 1.3 or later: * - * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... + * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... * * This also works during compile-time: * - * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) - * printf ("This is version 1.3 or later\n") ; - * #else - * printf ("This is an early version\n") ; - * #endif + * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) + * printf ("This is version 1.3 or later\n") ; + * #else + * printf ("This is an early version\n") ; + * #endif */ -#define CCOLAMD_DATE "Jan 25, 2011" +#define CCOLAMD_DATE "May 4, 2016" #define CCOLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub)) #define CCOLAMD_MAIN_VERSION 2 -#define CCOLAMD_SUB_VERSION 7 -#define CCOLAMD_SUBSUB_VERSION 3 +#define CCOLAMD_SUB_VERSION 9 +#define CCOLAMD_SUBSUB_VERSION 6 #define CCOLAMD_VERSION \ - CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) + CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) /* ========================================================================== */ /* === Knob and statistics definitions ====================================== */ @@ -94,106 +92,105 @@ extern "C" { #define CCOLAMD_NEWLY_EMPTY_COL 10 /* error codes returned in stats [3]: */ -#define CCOLAMD_OK (0) -#define CCOLAMD_OK_BUT_JUMBLED (1) -#define CCOLAMD_ERROR_A_not_present (-1) -#define CCOLAMD_ERROR_p_not_present (-2) -#define CCOLAMD_ERROR_nrow_negative (-3) -#define CCOLAMD_ERROR_ncol_negative (-4) -#define CCOLAMD_ERROR_nnz_negative (-5) -#define CCOLAMD_ERROR_p0_nonzero (-6) -#define CCOLAMD_ERROR_A_too_small (-7) -#define CCOLAMD_ERROR_col_length_negative (-8) -#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) -#define CCOLAMD_ERROR_out_of_memory (-10) -#define CCOLAMD_ERROR_invalid_cmember (-11) -#define CCOLAMD_ERROR_internal_error (-999) +#define CCOLAMD_OK (0) +#define CCOLAMD_OK_BUT_JUMBLED (1) +#define CCOLAMD_ERROR_A_not_present (-1) +#define CCOLAMD_ERROR_p_not_present (-2) +#define CCOLAMD_ERROR_nrow_negative (-3) +#define CCOLAMD_ERROR_ncol_negative (-4) +#define CCOLAMD_ERROR_nnz_negative (-5) +#define CCOLAMD_ERROR_p0_nonzero (-6) +#define CCOLAMD_ERROR_A_too_small (-7) +#define CCOLAMD_ERROR_col_length_negative (-8) +#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) +#define CCOLAMD_ERROR_out_of_memory (-10) +#define CCOLAMD_ERROR_invalid_cmember (-11) +#define CCOLAMD_ERROR_internal_error (-999) /* ========================================================================== */ /* === Prototypes of user-callable routines ================================= */ /* ========================================================================== */ -/* define UF_long */ -#include "UFconfig.h" +#include "SuiteSparse_config.h" -size_t ccolamd_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - int nnz, /* nonzeros in A */ - int n_row, /* number of rows in A */ - int n_col /* number of columns in A */ + int nnz, /* nonzeros in A */ + int n_row, /* number of rows in A */ + int n_col /* number of columns in A */ ) ; -size_t ccolamd_l_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_l_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - UF_long nnz, /* nonzeros in A */ - UF_long n_row, /* number of rows in A */ - UF_long n_col /* number of columns in A */ + SuiteSparse_long nnz, /* nonzeros in A */ + SuiteSparse_long n_row, /* number of rows in A */ + SuiteSparse_long n_col /* number of columns in A */ ) ; -void ccolamd_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -void ccolamd_l_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_l_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -int ccolamd /* returns (1) if successful, (0) otherwise*/ -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +int ccolamd /* returns (1) if successful, (0) otherwise*/ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ - int cmember [ ] /* Constraint set of A, of size n_col */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int cmember [ ] /* Constraint set of A, of size n_col */ ) ; -UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ +SuiteSparse_long ccolamd_l /* as ccolamd w/ SuiteSparse_long integers */ ( - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long A [ ], - UF_long p [ ], + SuiteSparse_long n_row, + SuiteSparse_long n_col, + SuiteSparse_long Alen, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], - UF_long cmember [ ] + SuiteSparse_long stats [CCOLAMD_STATS], + SuiteSparse_long cmember [ ] ) ; -int csymamd /* return (1) if OK, (0) otherwise */ +int csymamd /* return (1) if OK, (0) otherwise */ ( - int n, /* number of rows and columns of A */ - int A [ ], /* row indices of A */ - int p [ ], /* column pointers of A */ - int perm [ ], /* output permutation, size n_col+1 */ + int n, /* number of rows and columns of A */ + int A [ ], /* row indices of A */ + int p [ ], /* column pointers of A */ + int perm [ ], /* output permutation, size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameters (uses defaults if NULL) */ - int stats [CCOLAMD_STATS], /* output statistics and error codes */ + int stats [CCOLAMD_STATS], /* output statistics and error codes */ void * (*allocate) (size_t, size_t), /* pointer to calloc (ANSI C) or */ - /* mxCalloc (for MATLAB mexFunction) */ - void (*release) (void *), /* pointer to free (ANSI C) or */ - /* mxFree (for MATLAB mexFunction) */ - int cmember [ ], /* Constraint set of A */ - int stype /* 0: use both parts, >0: upper, <0: lower */ + /* mxCalloc (for MATLAB mexFunction) */ + void (*release) (void *), /* pointer to free (ANSI C) or */ + /* mxFree (for MATLAB mexFunction) */ + int cmember [ ], /* Constraint set of A */ + int stype /* 0: use both parts, >0: upper, <0: lower */ ) ; -UF_long csymamd_l /* same as csymamd, but with UF_long integers */ +SuiteSparse_long csymamd_l /* as csymamd, w/ SuiteSparse_long integers */ ( - UF_long n, - UF_long A [ ], - UF_long p [ ], - UF_long perm [ ], + SuiteSparse_long n, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], + SuiteSparse_long perm [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], + SuiteSparse_long stats [CCOLAMD_STATS], void * (*allocate) (size_t, size_t), void (*release) (void *), - UF_long cmember [ ], - UF_long stype + SuiteSparse_long cmember [ ], + SuiteSparse_long stype ) ; void ccolamd_report @@ -203,7 +200,7 @@ void ccolamd_report void ccolamd_l_report ( - UF_long stats [CCOLAMD_STATS] + SuiteSparse_long stats [CCOLAMD_STATS] ) ; void csymamd_report @@ -213,7 +210,7 @@ void csymamd_report void csymamd_l_report ( - UF_long stats [CCOLAMD_STATS] + SuiteSparse_long stats [CCOLAMD_STATS] ) ; @@ -227,42 +224,42 @@ void csymamd_l_report */ int ccolamd2 -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ /* each Front_ array is of size n_col+1: */ - int Front_npivcol [ ], /* # pivot cols in each front */ - int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ - int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ - int Front_parent [ ], /* parent of each front */ - int Front_cols [ ], /* link list of pivot columns for each front */ - int *p_nfr, /* total number of frontal matrices */ - int InFront [ ], /* InFront [row] = f if row in front f */ - int cmember [ ] /* Constraint set of A */ + int Front_npivcol [ ], /* # pivot cols in each front */ + int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ + int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ + int Front_parent [ ], /* parent of each front */ + int Front_cols [ ], /* link list of pivot columns for each front */ + int *p_nfr, /* total number of frontal matrices */ + int InFront [ ], /* InFront [row] = f if row in front f */ + int cmember [ ] /* Constraint set of A */ ) ; -UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */ +SuiteSparse_long ccolamd2_l /* as ccolamd2, w/ SuiteSparse_long integers */ ( - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long A [ ], - UF_long p [ ], + SuiteSparse_long n_row, + SuiteSparse_long n_col, + SuiteSparse_long Alen, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], - UF_long Front_npivcol [ ], - UF_long Front_nrows [ ], - UF_long Front_ncols [ ], - UF_long Front_parent [ ], - UF_long Front_cols [ ], - UF_long *p_nfr, - UF_long InFront [ ], - UF_long cmember [ ] + SuiteSparse_long stats [CCOLAMD_STATS], + SuiteSparse_long Front_npivcol [ ], + SuiteSparse_long Front_nrows [ ], + SuiteSparse_long Front_ncols [ ], + SuiteSparse_long Front_parent [ ], + SuiteSparse_long Front_cols [ ], + SuiteSparse_long *p_nfr, + SuiteSparse_long InFront [ ], + SuiteSparse_long cmember [ ] ) ; void ccolamd_apply_order @@ -276,11 +273,11 @@ void ccolamd_apply_order void ccolamd_l_apply_order ( - UF_long Front [ ], - const UF_long Order [ ], - UF_long Temp [ ], - UF_long nn, - UF_long nfr + SuiteSparse_long Front [ ], + const SuiteSparse_long Order [ ], + SuiteSparse_long Temp [ ], + SuiteSparse_long nn, + SuiteSparse_long nfr ) ; @@ -296,12 +293,12 @@ void ccolamd_fsize void ccolamd_l_fsize ( - UF_long nn, - UF_long MaxFsize [ ], - UF_long Fnrows [ ], - UF_long Fncols [ ], - UF_long Parent [ ], - UF_long Npiv [ ] + SuiteSparse_long nn, + SuiteSparse_long MaxFsize [ ], + SuiteSparse_long Fnrows [ ], + SuiteSparse_long Fncols [ ], + SuiteSparse_long Parent [ ], + SuiteSparse_long Npiv [ ] ) ; void ccolamd_postorder @@ -320,16 +317,16 @@ void ccolamd_postorder void ccolamd_l_postorder ( - UF_long nn, - UF_long Parent [ ], - UF_long Npiv [ ], - UF_long Fsize [ ], - UF_long Order [ ], - UF_long Child [ ], - UF_long Sibling [ ], - UF_long Stack [ ], - UF_long Front_cols [ ], - UF_long cmember [ ] + SuiteSparse_long nn, + SuiteSparse_long Parent [ ], + SuiteSparse_long Npiv [ ], + SuiteSparse_long Fsize [ ], + SuiteSparse_long Order [ ], + SuiteSparse_long Child [ ], + SuiteSparse_long Sibling [ ], + SuiteSparse_long Stack [ ], + SuiteSparse_long Front_cols [ ], + SuiteSparse_long cmember [ ] ) ; int ccolamd_post_tree @@ -342,22 +339,16 @@ int ccolamd_post_tree int Stack [ ] ) ; -UF_long ccolamd_l_post_tree +SuiteSparse_long ccolamd_l_post_tree ( - UF_long root, - UF_long k, - UF_long Child [ ], - const UF_long Sibling [ ], - UF_long Order [ ], - UF_long Stack [ ] + SuiteSparse_long root, + SuiteSparse_long k, + SuiteSparse_long Child [ ], + const SuiteSparse_long Sibling [ ], + SuiteSparse_long Order [ ], + SuiteSparse_long Stack [ ] ) ; -#ifndef EXTERN -#define EXTERN extern -#endif - -EXTERN int (*ccolamd_printf) (const char *, ...) ; - #ifdef __cplusplus } #endif diff --git a/gtsam/3rdparty/CCOLAMD/Lib/Makefile b/gtsam/3rdparty/CCOLAMD/Lib/Makefile new file mode 100644 index 000000000..52c52eb9e --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Lib/Makefile @@ -0,0 +1,73 @@ +#------------------------------------------------------------------------------- +# CCOLAMD Lib/Makefile +#------------------------------------------------------------------------------- + +LIBRARY = libccolamd +VERSION = 2.9.6 +SO_VERSION = 2 + +default: library + +include ../../SuiteSparse_config/SuiteSparse_config.mk + +# CCOLAMD depends on SuiteSparse_config +LDLIBS += -lsuitesparseconfig + +# compile and install in SuiteSparse/lib +library: + $(MAKE) install INSTALL=$(SUITESPARSE) + +I = -I../Include -I../../SuiteSparse_config + +INC = ../Include/ccolamd.h ../../SuiteSparse_config/SuiteSparse_config.h + +SRC = ../Source/ccolamd.c + +OBJ = ccolamd.o ccolamd_l.o + +ccolamd.o: $(SRC) $(INC) + $(CC) $(CF) $(I) -c ../Source/ccolamd.c + +ccolamd_l.o: $(SRC) $(INC) + $(CC) $(CF) $(I) -c ../Source/ccolamd.c -DDLONG -o ccolamd_l.o + +# creates libccolamd.a, a C-callable CCOLAMD library +static: $(AR_TARGET) + +$(AR_TARGET): $(OBJ) + $(ARCHIVE) $@ $^ + - $(RANLIB) $@ + +ccode: library + +clean: + - $(RM) -r $(CLEAN) + +purge: distclean + +distclean: clean + - $(RM) -r $(PURGE) + +# install CCOLAMD +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(OBJ) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) ../Include/ccolamd.h $(INSTALL_INCLUDE) + $(CP) ../README.txt $(INSTALL_DOC)/CCOLAMD_README.txt + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 644 $(INSTALL_INCLUDE)/ccolamd.h + chmod 644 $(INSTALL_DOC)/CCOLAMD_README.txt + +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_LIB)/$(SO_MAIN) + $(RM) $(INSTALL_INCLUDE)/ccolamd.h + $(RM) $(INSTALL_DOC)/CCOLAMD_README.txt + diff --git a/gtsam/3rdparty/CCOLAMD/Lib/libccolamd.a b/gtsam/3rdparty/CCOLAMD/Lib/libccolamd.a deleted file mode 100644 index dc4502e9e..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/libccolamd.a and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m index e2fd68457..2c69a502e 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m @@ -47,9 +47,8 @@ spparms ('default') ; A = sprandn (n, n, 2/n) + speye (n) ; b = (1:n)' ; -figure (1) clf ; -subplot (2,2,1) +subplot (3,4,1) spy (A) title ('original matrix') @@ -62,7 +61,7 @@ fl = luflops (L, U) ; x = Q * (U \ (L \ (P * b))) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,2) ; +subplot (3,4,2) ; spy (L|U) ; title ('LU with ccolamd') ; @@ -76,7 +75,7 @@ fl = luflops (L, U) ; x = Q * (U \ (L \ (P * b))) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,3) ; +subplot (3,4,3) ; spy (L|U) ; title ('LU with colamd') ; catch @@ -89,7 +88,7 @@ fl = luflops (L, U) ; x = U \ (L \ (P * b)) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,4) ; +subplot (3,4,4) ; spy (L|U) ; title ('LU with no ordering') ; @@ -111,9 +110,7 @@ n = 1000 ; fprintf (1, 'Generating a random %d-by-%d sparse matrix.\n', n, n) ; A = sprandn (n, n, 2/n) + speye (n) ; -figure (2) -clf ; -subplot (2,2,1) +subplot (3,4,5) spy (A) title ('original matrix') @@ -121,7 +118,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ; [lnz,h,parent,post,R] = symbfact (A, 'col') ; fprintf (1, 'nz in Cholesky factors of A''A: %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A''A: %d\n', sum (lnz.^2)) ; -subplot (2,2,4) ; +subplot (3,4,6) ; spy (R) ; title ('Cholesky with no ordering') ; @@ -133,7 +130,7 @@ fprintf (1, '\n\nccolamd run time: %f\n', t) ; fprintf (1, 'ccolamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,2) ; +subplot (3,4,7) ; spy (R) ; title ('Cholesky with ccolamd') ; @@ -146,7 +143,7 @@ fprintf (1, '\n\ncolamd run time: %f\n', t) ; fprintf (1, 'colamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,3) ; +subplot (3,4,8) ; spy (R) ; title ('Cholesky with colamd') ; catch @@ -164,9 +161,7 @@ fprintf (1, '\n-----------------------------------------------------------\n') ; fprintf (1, 'Generating a random symmetric %d-by-%d sparse matrix.\n', n, n) ; A = A+A' ; -figure (3) -clf ; -subplot (2,2,1) +subplot (3,4,9) ; spy (A) title ('original matrix') @@ -174,7 +169,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ; [lnz,h,parent,post,R] = symbfact (A, 'sym') ; fprintf (1, 'nz in Cholesky factors of A: %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A: %d\n', sum (lnz.^2)) ; -subplot (2,2,4) ; +subplot (3,4,10) ; spy (R) ; title ('Cholesky with no ordering') ; @@ -186,7 +181,7 @@ fprintf (1, '\n\ncsymamd run time: %f\n', t) ; fprintf (1, 'csymamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,2) ; +subplot (3,4,11) ; spy (R) ; title ('Cholesky with csymamd') ; @@ -199,7 +194,7 @@ fprintf (1, '\n\nsymamd run time: %f\n', t) ; fprintf (1, 'symamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,3) ; +subplot (3,4,12) ; spy (R) ; title ('Cholesky with symamd') ; catch diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m index 4637f3884..9a84da909 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m @@ -14,14 +14,33 @@ d = '' ; if (~isempty (strfind (computer, '64'))) d = '-largeArrayDims' ; end -src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ; -cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include -output ', d) ; + +% MATLAB 8.3.0 now has a -silent option to keep 'mex' from burbling too much +if (~verLessThan ('matlab', '8.3.0')) + d = ['-silent ' d] ; +end + +src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ; +cmd = sprintf ( ... + 'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include -output ', d) ; s = [cmd 'ccolamd ccolamdmex.c ' src] ; + +if (~(ispc || ismac)) + % for POSIX timing routine + s = [s ' -lrt'] ; +end if (details) fprintf ('%s\n', s) ; end eval (s) ; + s = [cmd 'csymamd csymamdmex.c ' src] ; + +if (~(ispc || ismac)) + % for POSIX timing routine + s = [s ' -lrt'] ; +end + if (details) fprintf ('%s\n', s) ; end diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m index e359b9fea..0514ad9c7 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m @@ -22,8 +22,13 @@ csymamd_default_knobs = [10 1 0] ; if (~isempty (strfind (computer, '64'))) d = '-largeArrayDims' ; end - src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ; - cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include ', d) ; + cmd = sprintf ( ... + 'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include ', d) ; + src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ; + if (~(ispc || ismac)) + % for POSIX timing routine + src = [src ' -lrt'] ; + end eval ([cmd 'ccolamdtestmex.c ' src]) ; eval ([cmd 'csymamdtestmex.c ' src]) ; fprintf ('Done compiling.\n') ; diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c index fbcb87873..09d21efee 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -26,7 +24,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* ========================================================================== */ /* === ccolamd mexFunction ================================================== */ @@ -44,24 +42,24 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* ccolamd's copy of the matrix and workspace */ - UF_long *cmember ; /* ccolamd's copy of the constraint set */ - double *in_cmember ; /* input constraint set */ - UF_long *p ; /* ccolamd's copy of the column pointers */ - UF_long Alen ; /* size of A */ - UF_long cslen ; /* size of CS */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long nnz ; /* number of entries in A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* ccolamd's copy of the matrix and workspace */ + Long *cmember ; /* ccolamd's copy of the constraint set */ + double *in_cmember ; /* input constraint set */ + Long *p ; /* ccolamd's copy of the column pointers */ + Long Alen ; /* size of A */ + Long cslen ; /* size of CS */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long nnz ; /* number of entries in A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats [CCOLAMD_STATS] ; /* stats for ccolamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats [CCOLAMD_STATS] ;/* stats for ccolamd */ /* === Check inputs ===================================================== */ @@ -80,11 +78,11 @@ void mexFunction cslen = mxGetNumberOfElements (pargin [2]) ; if (cslen != 0) { - cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ; + cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ; for (i = 0 ; i < cslen ; i++) { /* convert cmember from 1-based to 0-based */ - cmember[i] = ((UF_long) in_cmember [i] - 1) ; + cmember[i] = ((Long) in_cmember [i] - 1) ; } } } @@ -157,10 +155,10 @@ void mexFunction n_col = mxGetN (Ainput) ; /* get column pointer vector */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; - Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ; + Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ; if (Alen == 0) { mexErrMsgTxt ("ccolamd: problem too large") ; @@ -168,8 +166,8 @@ void mexFunction /* === Copy input matrix into workspace ================================= */ - A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (Alen, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; if (full) { diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c index b5b04da73..18c850dc6 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -43,7 +41,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* Here only for testing */ #undef MIN @@ -61,15 +59,15 @@ static void dump_matrix ( - UF_long A [ ], - UF_long p [ ], - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long limit + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit ) { - UF_long col, k, row ; + Long col, k, row ; mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ; @@ -102,24 +100,24 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* ccolamd's copy of the matrix and workspace */ - UF_long *p ; /* ccolamd's copy of the column pointers */ - UF_long Alen ; /* size of A */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long nnz ; /* number of entries in A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* ccolamd's copy of the matrix and workspace */ + Long *p ; /* ccolamd's copy of the column pointers */ + Long Alen ; /* size of A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long nnz ; /* number of entries in A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */ - UF_long *cp, *cp_end, result, col, length, ok ; - UF_long *stats ; + Long *cp, *cp_end, result, col, length, ok ; + Long *stats ; stats = stats2 ; /* === Check inputs ===================================================== */ @@ -199,10 +197,10 @@ void mexFunction n_col = mxGetN (Ainput) ; /* get column pointer vector so we can find nnz */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; - Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ; + Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ; if (Alen == 0) { mexErrMsgTxt ("ccolamd: problem too large") ; @@ -230,8 +228,8 @@ void mexFunction /* === Copy input matrix into workspace ================================= */ - A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (Alen, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; if (full) { @@ -261,7 +259,7 @@ void mexFunction */ /* jumble appropriately */ - switch ((UF_long) in_knobs [6]) + switch ((Long) in_knobs [6]) { case 0 : @@ -359,7 +357,7 @@ void mexFunction mexPrintf ("ccolamdtest: A not present\n") ; } result = 0 ; /* A not present */ - A = (UF_long *) NULL ; + A = (Long *) NULL ; break ; case 8 : @@ -368,7 +366,7 @@ void mexFunction mexPrintf ("ccolamdtest: p not present\n") ; } result = 0 ; /* p not present */ - p = (UF_long *) NULL ; + p = (Long *) NULL ; break ; case 9 : @@ -456,7 +454,7 @@ void mexFunction mexPrintf ("ccolamdtest: stats not present\n") ; } result = 0 ; /* stats not present */ - stats = (UF_long *) NULL ; + stats = (Long *) NULL ; break ; case 13 : diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m index 901040df6..4308f9934 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m @@ -34,10 +34,10 @@ function [p, stats] = csymamd (S, knobs, cmember) %#ok % p = csymamd(S) is about the same as p = symamd(S). knobs and its default % values differ. % -% Authors: S. Larimore, T. Davis (Univ of Florida), and S. Rajamanickam, in +% Authors: S. Larimore, T. Davis, and S. Rajamanickam, in % collaboration with J. Gilbert and E. Ng. Supported by the National % Science Foundation (DMS-9504974, DMS-9803599, CCR-0203270), and a grant -% from Sandia National Lab. See http://www.cise.ufl.edu/research/sparse +% from Sandia National Lab. See http://www.suitesparse.com % for ccolamd, csymamd, amd, colamd, symamd, and other related orderings. % % See also AMD, CCOLAMD, COLAMD, SYMAMD, SYMRCM. diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c index e52d92619..41a3b4346 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -25,7 +23,7 @@ #include "mex.h" #include "matrix.h" #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* ========================================================================== */ /* === csymamd mexFunction ================================================== */ @@ -43,23 +41,23 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* row indices of input matrix A */ - UF_long *perm ; /* column ordering of M and ordering of A */ - UF_long *cmember ; /* csymamd's copy of the constraint set */ - double *in_cmember ; /* input constraint set */ - UF_long *p ; /* column pointers of input matrix A */ - UF_long cslen ; /* size of constraint set */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* row indices of input matrix A */ + Long *perm ; /* column ordering of M and ordering of A */ + Long *cmember ; /* csymamd's copy of the constraint set */ + double *in_cmember ; /* input constraint set */ + Long *p ; /* column pointers of input matrix A */ + Long cslen ; /* size of constraint set */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* csymamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats [CCOLAMD_STATS] ; /* stats for symamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats [CCOLAMD_STATS] ;/* stats for symamd */ /* === Check inputs ===================================================== */ @@ -78,11 +76,11 @@ void mexFunction cslen = mxGetNumberOfElements (pargin [2]) ; if (cslen != 0) { - cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ; + cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ; for (i = 0 ; i < cslen ; i++) { /* convert cmember from 1-based to 0-based */ - cmember[i] = ((UF_long) in_cmember [i] - 1) ; + cmember[i] = ((Long) in_cmember [i] - 1) ; } } } @@ -153,9 +151,9 @@ void mexFunction mexErrMsgTxt ("csymamd: cmember must be of length equal to #cols of A"); } - A = (UF_long *) mxGetIr (Ainput) ; - p = (UF_long *) mxGetJc (Ainput) ; - perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; + A = (Long *) mxGetIr (Ainput) ; + p = (Long *) mxGetJc (Ainput) ; + perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; /* === Order the rows and columns of A (does not destroy A) ============= */ diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c index 428b41185..60b0d6c04 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -37,7 +35,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long #ifdef MIN #undef MIN @@ -47,15 +45,15 @@ static void dump_matrix ( - UF_long A [ ], - UF_long p [ ], - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long limit + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit ) { - UF_long col, k, row ; + Long col, k, row ; mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ; @@ -100,23 +98,23 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *perm ; /* column ordering of M and ordering of A */ - UF_long *A ; /* row indices of input matrix A */ - UF_long *p ; /* column pointers of input matrix A */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *perm ; /* column ordering of M and ordering of A */ + Long *A ; /* row indices of input matrix A */ + Long *p ; /* column pointers of input matrix A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */ - UF_long *cp, *cp_end, result, nnz, col, length, ok ; - UF_long *stats ; + Long *cp, *cp_end, result, nnz, col, length, ok ; + Long *stats ; stats = stats2 ; /* === Check inputs ===================================================== */ @@ -192,8 +190,8 @@ void mexFunction } /* p = mxGetJc (Ainput) ; */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; if (spumoni) @@ -202,10 +200,10 @@ void mexFunction } /* A = mxGetIr (Ainput) ; */ - A = (UF_long *) mxCalloc (nnz+1, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (nnz+1, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; - perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; + perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; /* === Jumble matrix ==================================================== */ @@ -230,7 +228,7 @@ void mexFunction */ /* jumble appropriately */ - switch ((UF_long) in_knobs [3]) + switch ((Long) in_knobs [3]) { case 0 : @@ -321,7 +319,7 @@ void mexFunction mexPrintf ("csymamdtest: A not present\n") ; } result = 0 ; /* A not present */ - A = (UF_long *) NULL ; + A = (Long *) NULL ; break ; case 8 : @@ -330,7 +328,7 @@ void mexFunction mexPrintf ("csymamdtest: p not present\n") ; } result = 0 ; /* p not present */ - p = (UF_long *) NULL ; + p = (Long *) NULL ; break ; case 9 : @@ -418,7 +416,7 @@ void mexFunction mexPrintf ("csymamdtest: stats not present\n") ; } result = 0 ; /* stats not present */ - stats = (UF_long *) NULL ; + stats = (Long *) NULL ; break ; case 13 : diff --git a/gtsam/3rdparty/CCOLAMD/Makefile b/gtsam/3rdparty/CCOLAMD/Makefile new file mode 100644 index 000000000..ecebb8b92 --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Makefile @@ -0,0 +1,55 @@ +#------------------------------------------------------------------------------ +# CCOLAMD Makefile +#------------------------------------------------------------------------------ + +SUITESPARSE ?= $(realpath $(CURDIR)/..) +export SUITESPARSE + +default: all + +include ../SuiteSparse_config/SuiteSparse_config.mk + +demos: all + +# Compile all C code +all: + ( cd Lib ; $(MAKE) ) + ( cd Demo ; $(MAKE) ) + +# compile just the C-callable libraries (not Demos) +library: + ( cd Lib ; $(MAKE) ) + +# compile the static libraries only +static: + ( cd Lib ; $(MAKE) static ) + +# remove object files, but keep the compiled programs and library archives +clean: + ( cd Lib ; $(MAKE) clean ) + ( cd Demo ; $(MAKE) clean ) + ( cd MATLAB ; $(RM) $(CLEAN) ) + +# clean, and then remove compiled programs and library archives +purge: + ( cd Lib ; $(MAKE) purge ) + ( cd Demo ; $(MAKE) purge ) + ( cd MATLAB ; $(RM) $(CLEAN) ; $(RM) *.mex* ) + +distclean: purge + +# get ready for distribution +dist: purge + ( cd Demo ; $(MAKE) dist ) + +ccode: library + +lib: library + +# install CCOLAMD +install: + ( cd Lib ; $(MAKE) install ) + +# uninstall CCOLAMD +uninstall: + ( cd Lib ; $(MAKE) uninstall ) diff --git a/gtsam/3rdparty/CCOLAMD/README.txt b/gtsam/3rdparty/CCOLAMD/README.txt index ccc5a19a5..6d8f1ce83 100644 --- a/gtsam/3rdparty/CCOLAMD/README.txt +++ b/gtsam/3rdparty/CCOLAMD/README.txt @@ -1,8 +1,8 @@ CCOLAMD: constrained column approximate minimum degree ordering -Copyright (C) 2005-2011, Univ. of Florida. Authors: Timothy A. Davis, +Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. -http://www.cise.ufl.edu/research/sparse +http://www.suitesparse.com ------------------------------------------------------------------------------- The CCOLAMD column approximate minimum degree ordering algorithm computes @@ -14,7 +14,8 @@ available as a MATLAB-callable function. It constructs a matrix M such that M'*M has the same pattern as A, and then uses CCOLAMD to compute a column ordering of M. -Requires UFconfig, in the ../UFconfig directory relative to this directory. +Requires SuiteSparse_config, in the ../SuiteSparse_config directory relative to +this directory. To compile and install the ccolamd m-files and mexFunctions, just cd to CCOLAMD/MATLAB and type ccolamd_install in the MATLAB command window. @@ -22,47 +23,27 @@ A short demo will run. Optionally, type ccolamd_test to run an extensive tests. Type "make" in Unix in the CCOLAMD directory to compile the C-callable library and to run a short demo. -If you have MATLAB 7.2 or earlier, you must first edit UFconfig/UFconfig.h to -remove the "-largeArrayDims" option from the MEX command (or just use -ccolamd_install.m inside MATLAB). - Other "make" targets: - make mex compiles MATLAB mexFunctions only - make libccolamd.a compiles a C-callable library containing ccolamd - make clean removes all files not in the distribution, except for - libccolamd.a + make library compiles a C-callable library containing ccolamd + make clean removes all files not in the distribution + but keeps the compiled libraries. make distclean removes all files not in the distribution + make install installs the library in /usr/local/lib and + /usr/local/include + make uninstall uninstalls the library from /usr/local/lib and + /usr/local/include To use ccolamd and csymamd within an application written in C, all you need are ccolamd.c and ccolamd.h, which are the C-callable ccolamd/csymamd codes. See ccolamd.c for more information on how to call ccolamd from a C program. It contains a complete description of the C-interface to CCOLAMD and CSYMAMD. - Copyright (c) 1998-2007 by the University of Florida. - All Rights Reserved. - Licensed under the GNU LESSER GENERAL PUBLIC LICENSE. +See CCOLAMD/Doc/License.txt for the license. ------------------------------------------------------------------------------- -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - -------------------------------------------------------------------------------- - - Related papers: T. A. Davis and W. W. Hager, Rajamanickam, Multiple-rank updates @@ -86,21 +67,18 @@ Related papers: "An approximate minimum degree column ordering algorithm", S. I. Larimore, MS Thesis, Dept. of Computer and Information Science and Engineering, University of Florida, Gainesville, FL, - 1998. CISE Tech Report TR-98-016. Available at - ftp://ftp.cise.ufl.edu/cis/tech-reports/tr98/tr98-016.ps - via anonymous ftp. + 1998. CISE Tech Report TR-98-016. Approximate Deficiency for Ordering the Columns of a Matrix, J. L. Kern, Senior Thesis, Dept. of Computer and Information Science and Engineering, University of Florida, Gainesville, FL, - 1999. Available at http://www.cise.ufl.edu/~davis/Kern/kern.ps + 1999. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Stefan I. Larimore and Timothy A. Davis, - University of Florida, in collaboration with John Gilbert, Xerox PARC - (now at UC Santa Barbara), and Esmong Ng, Lawrence Berkeley National - Laboratory (much of this work he did while at Oak Ridge National - Laboratory). + in collaboration with John Gilbert, Xerox PARC (now at UC Santa + Barbara), and Esmong Ng, Lawrence Berkeley National Laboratory (much of + this work he did while at Oak Ridge National Laboratory). CCOLAMD files: @@ -122,7 +100,6 @@ CCOLAMD files: ./Doc: ChangeLog change log - lesser.txt license ./Include: ccolamd.h include file @@ -147,4 +124,3 @@ CCOLAMD files: ./Source: ccolamd.c primary source code - ccolamd_global.c globally defined function pointers (malloc, free, ...) diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 1c35ffa41..9a08e3ea8 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C) Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -58,39 +56,13 @@ * COLAMD is also available under alternate licenses, contact T. Davis * for details. * - * This library is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * This library is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 - * USA - * - * Permission is hereby granted to use or copy this program under the - * terms of the GNU LGPL, provided that the Copyright, this License, - * and the Availability of the original version is retained on all copies. - * User documentation of any code that uses this code or any modified - * version of this code must cite the Copyright, this License, the - * Availability note, and "Used by permission." Permission to modify - * the code and to distribute modified code is granted, provided the - * Copyright, this License, and the Availability note are retained, - * and a notice that the code was modified is included. + * See CCOLAMD/Doc/License.txt for the license. * * Availability: * * The CCOLAMD/CSYMAMD library is available at * - * http://www.cise.ufl.edu/research/sparse/ccolamd/ - * - * This is the http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd.c - * file. + * http://www.suitesparse.com * * See the ChangeLog file for changes since Version 1.0. */ @@ -99,10 +71,10 @@ /* === Description of user-callable routines ================================ */ /* ========================================================================== */ -/* CCOLAMD includes both int and UF_long versions of all its routines. The - * description below is for the int version. For UF_long, all int arguments - * become UF_long integers. UF_long is normally defined as long, except for - * WIN64 */ +/* CCOLAMD includes both int and SuiteSparse_long versions of all its routines. + * The description below is for the int version. For SuiteSparse_long, all + * int arguments become SuiteSparse_long integers. SuiteSparse_long is + * normally defined as long, except for WIN64 */ /* ---------------------------------------------------------------------------- * ccolamd_recommended: @@ -112,8 +84,8 @@ * * #include "ccolamd.h" * size_t ccolamd_recommended (int nnz, int n_row, int n_col) ; - * size_t ccolamd_l_recommended (UF_long nnz, UF_long n_row, - * UF_long n_col) ; + * size_t ccolamd_l_recommended (SuiteSparse_long nnz, + * SuiteSparse_long n_row, SuiteSparse_long n_col) ; * * Purpose: * @@ -209,9 +181,12 @@ * double knobs [CCOLAMD_KNOBS], int stats [CCOLAMD_STATS], * int *cmember) ; * - * UF_long ccolamd_l (UF_long n_row, UF_long n_col, UF_long Alen, - * UF_long *A, UF_long *p, double knobs [CCOLAMD_KNOBS], - * UF_long stats [CCOLAMD_STATS], UF_long *cmember) ; + * SuiteSparse_long ccolamd_l (SuiteSparse_long n_row, + * SuiteSparse_long n_col, SuiteSparse_long Alen, + * SuiteSparse_long *A, SuiteSparse_long *p, + * double knobs [CCOLAMD_KNOBS], + * SuiteSparse_long stats [CCOLAMD_STATS], + * SuiteSparse_long *cmember) ; * * Purpose: * @@ -385,9 +360,7 @@ * * Example: * - * See - * http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd_example.c - * for a complete example. + * See ccolamd_example.c for a complete example. * * To order the columns of a 5-by-4 matrix with 11 nonzero entries in * the following nonzero pattern @@ -423,10 +396,12 @@ * void (*allocate) (size_t, size_t), void (*release) (void *), * int *cmember, int stype) ; * - * UF_long csymamd_l (UF_long n, UF_long *A, UF_long *p, UF_long *perm, - * double knobs [CCOLAMD_KNOBS], UF_long stats [CCOLAMD_STATS], - * void (*allocate) (size_t, size_t), void (*release) (void *), - * UF_long *cmember, UF_long stype) ; + * SuiteSparse_long csymamd_l (SuiteSparse_long n, + * SuiteSparse_long *A, SuiteSparse_long *p, + * SuiteSparse_long *perm, double knobs [CCOLAMD_KNOBS], + * SuiteSparse_long stats [CCOLAMD_STATS], void (*allocate) + * (size_t, size_t), void (*release) (void *), + * SuiteSparse_long *cmember, SuiteSparse_long stype) ; * * Purpose: * @@ -562,7 +537,7 @@ * * #include "ccolamd.h" * ccolamd_report (int stats [CCOLAMD_STATS]) ; - * ccolamd_l_report (UF_long stats [CCOLAMD_STATS]) ; + * ccolamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ; * * Purpose: * @@ -583,7 +558,7 @@ * * #include "ccolamd.h" * csymamd_report (int stats [CCOLAMD_STATS]) ; - * csymamd_l_report (UF_long stats [CCOLAMD_STATS]) ; + * csymamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ; * * Purpose: * @@ -617,12 +592,11 @@ #include "ccolamd.h" +#include #include #include #ifdef MATLAB_MEX_FILE -#include -typedef uint16_t char16_t; #include "mex.h" #include "matrix.h" #endif @@ -636,17 +610,14 @@ typedef uint16_t char16_t; #endif /* ========================================================================== */ -/* === int or UF_long ======================================================= */ +/* === int or SuiteSparse_long ============================================== */ /* ========================================================================== */ -/* define UF_long */ -#include "UFconfig.h" - #ifdef DLONG -#define Int UF_long -#define ID UF_long_id -#define Int_MAX UF_long_max +#define Int SuiteSparse_long +#define ID SuiteSparse_long_id +#define Int_MAX SuiteSparse_long_max #define CCOLAMD_recommended ccolamd_l_recommended #define CCOLAMD_set_defaults ccolamd_l_set_defaults @@ -811,9 +782,6 @@ typedef struct CColamd_Row_struct #define INDEX(i) (i) #endif -/* All output goes through the PRINTF macro. */ -#define PRINTF(params) { if (ccolamd_printf != NULL) (void) ccolamd_printf params ; } - /* ========================================================================== */ /* === Debugging prototypes and definitions ================================= */ @@ -827,11 +795,11 @@ typedef struct CColamd_Row_struct PRIVATE Int ccolamd_debug ; /* debug print statements */ -#define DEBUG0(params) { PRINTF (params) ; } -#define DEBUG1(params) { if (ccolamd_debug >= 1) PRINTF (params) ; } -#define DEBUG2(params) { if (ccolamd_debug >= 2) PRINTF (params) ; } -#define DEBUG3(params) { if (ccolamd_debug >= 3) PRINTF (params) ; } -#define DEBUG4(params) { if (ccolamd_debug >= 4) PRINTF (params) ; } +#define DEBUG0(params) { SUITESPARSE_PRINTF (params) ; } +#define DEBUG1(params) { if (ccolamd_debug >= 1) SUITESPARSE_PRINTF (params) ; } +#define DEBUG2(params) { if (ccolamd_debug >= 2) SUITESPARSE_PRINTF (params) ; } +#define DEBUG3(params) { if (ccolamd_debug >= 3) SUITESPARSE_PRINTF (params) ; } +#define DEBUG4(params) { if (ccolamd_debug >= 4) SUITESPARSE_PRINTF (params) ; } #ifdef MATLAB_MEX_FILE #define ASSERT(expression) (mxAssert ((expression), "")) @@ -3752,12 +3720,12 @@ PRIVATE void print_report Int i1, i2, i3 ; - PRINTF (("\n%s version %d.%d, %s: ", method, + SUITESPARSE_PRINTF (("\n%s version %d.%d, %s: ", method, CCOLAMD_MAIN_VERSION, CCOLAMD_SUB_VERSION, CCOLAMD_DATE)) ; if (!stats) { - PRINTF (("No statistics available.\n")) ; + SUITESPARSE_PRINTF (("No statistics available.\n")) ; return ; } @@ -3767,11 +3735,11 @@ PRIVATE void print_report if (stats [CCOLAMD_STATUS] >= 0) { - PRINTF(("OK. ")) ; + SUITESPARSE_PRINTF(("OK. ")) ; } else { - PRINTF(("ERROR. ")) ; + SUITESPARSE_PRINTF(("ERROR. ")) ; } switch (stats [CCOLAMD_STATUS]) @@ -3779,91 +3747,105 @@ PRIVATE void print_report case CCOLAMD_OK_BUT_JUMBLED: - PRINTF(("Matrix has unsorted or duplicate row indices.\n")) ; + SUITESPARSE_PRINTF(( + "Matrix has unsorted or duplicate row indices.\n")) ; - PRINTF(("%s: duplicate or out-of-order row indices: "ID"\n", - method, i3)) ; + SUITESPARSE_PRINTF(( + "%s: duplicate or out-of-order row indices: "ID"\n", + method, i3)) ; - PRINTF(("%s: last seen duplicate or out-of-order row: "ID"\n", - method, INDEX (i2))) ; + SUITESPARSE_PRINTF(( + "%s: last seen duplicate or out-of-order row: "ID"\n", + method, INDEX (i2))) ; - PRINTF(("%s: last seen in column: "ID"", - method, INDEX (i1))) ; + SUITESPARSE_PRINTF(( + "%s: last seen in column: "ID"", + method, INDEX (i1))) ; /* no break - fall through to next case instead */ case CCOLAMD_OK: - PRINTF(("\n")) ; + SUITESPARSE_PRINTF(("\n")) ; - PRINTF(("%s: number of dense or empty rows ignored: "ID"\n", - method, stats [CCOLAMD_DENSE_ROW])) ; + SUITESPARSE_PRINTF(( + "%s: number of dense or empty rows ignored: "ID"\n", + method, stats [CCOLAMD_DENSE_ROW])) ; - PRINTF(("%s: number of dense or empty columns ignored: "ID"\n", - method, stats [CCOLAMD_DENSE_COL])) ; + SUITESPARSE_PRINTF(( + "%s: number of dense or empty columns ignored: "ID"\n", + method, stats [CCOLAMD_DENSE_COL])) ; - PRINTF(("%s: number of garbage collections performed: "ID"\n", - method, stats [CCOLAMD_DEFRAG_COUNT])) ; + SUITESPARSE_PRINTF(( + "%s: number of garbage collections performed: "ID"\n", + method, stats [CCOLAMD_DEFRAG_COUNT])) ; break ; case CCOLAMD_ERROR_A_not_present: - PRINTF(("Array A (row indices of matrix) not present.\n")) ; + SUITESPARSE_PRINTF(( + "Array A (row indices of matrix) not present.\n")) ; break ; case CCOLAMD_ERROR_p_not_present: - PRINTF(("Array p (column pointers for matrix) not present.\n")) ; + SUITESPARSE_PRINTF(( + "Array p (column pointers for matrix) not present.\n")) ; break ; case CCOLAMD_ERROR_nrow_negative: - PRINTF(("Invalid number of rows ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(("Invalid number of rows ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_ncol_negative: - PRINTF(("Invalid number of columns ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(("Invalid number of columns ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_nnz_negative: - PRINTF(("Invalid number of nonzero entries ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(( + "Invalid number of nonzero entries ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_p0_nonzero: - PRINTF(("Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ; + SUITESPARSE_PRINTF(( + "Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ; break ; case CCOLAMD_ERROR_A_too_small: - PRINTF(("Array A too small.\n")) ; - PRINTF((" Need Alen >= "ID", but given only Alen = "ID".\n", - i1, i2)) ; + SUITESPARSE_PRINTF(("Array A too small.\n")) ; + SUITESPARSE_PRINTF(( + " Need Alen >= "ID", but given only Alen = "ID".\n", + i1, i2)) ; break ; case CCOLAMD_ERROR_col_length_negative: - PRINTF(("Column "ID" has a negative number of entries ("ID").\n", - INDEX (i1), i2)) ; + SUITESPARSE_PRINTF(( + "Column "ID" has a negative number of entries ("ID").\n", + INDEX (i1), i2)) ; break ; case CCOLAMD_ERROR_row_index_out_of_bounds: - PRINTF(("Row index (row "ID") out of bounds ("ID" to "ID") in" - "column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1), - INDEX (i1))) ; + SUITESPARSE_PRINTF(( + "Row index (row "ID") out of bounds ("ID" to "ID") in" + "column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1), + INDEX (i1))) ; break ; case CCOLAMD_ERROR_out_of_memory: - PRINTF(("Out of memory.\n")) ; + SUITESPARSE_PRINTF(("Out of memory.\n")) ; break ; case CCOLAMD_ERROR_invalid_cmember: - PRINTF(("cmember invalid\n")) ; + SUITESPARSE_PRINTF(("cmember invalid\n")) ; break ; } } diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c deleted file mode 100644 index e470804a6..000000000 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c +++ /dev/null @@ -1,28 +0,0 @@ -/* ========================================================================== */ -/* === ccolamd_global.c ===================================================== */ -/* ========================================================================== */ - -/* ---------------------------------------------------------------------------- - * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, - * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse - * -------------------------------------------------------------------------- */ - -/* Global variables for CCOLAMD */ - -#ifndef NPRINT -#ifdef MATLAB_MEX_FILE -#include -#include -typedef uint16_t char16_t; -#include "mex.h" -int (*ccolamd_printf) (const char *, ...) = mexPrintf ; -#else -#include -int (*ccolamd_printf) (const char *, ...) = printf ; -#endif -#else -int (*ccolamd_printf) (const char *, ...) = ((void *) 0) ; -#endif - diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 8534a8d7e..b890bc4af 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -1,6 +1,6 @@ # install CCOLAMD headers install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD) -install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig) +install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config) if(NOT GTSAM_USE_SYSTEM_EIGEN) # Find plain .h files diff --git a/gtsam/3rdparty/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt index c4115d7f6..4dd5bd180 100644 --- a/gtsam/3rdparty/Eigen/.hg_archival.txt +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15 +node: b9cd8366d4e8f49471c7afafc4c2a1b00e54a54d branch: 3.2 -tag: 3.2.8 +tag: 3.2.10 diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags index 8f0097f20..c83128570 100644 --- a/gtsam/3rdparty/Eigen/.hgtags +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -31,3 +31,5 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0 bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5 c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6 b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7 +07105f7124f9aef00a68c85e0fc606e65d3d6c15 3.2.8 +dc6cfdf9bcec5efc7b6593bddbbb3d675de53524 3.2.9 diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h index 827894443..87bedfa46 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h @@ -66,9 +66,8 @@ struct traits > : traits::MaxColsAtCompileTime), XprTypeIsRowMajor = (int(traits::Flags)&RowMajorBit) != 0, - IsDense = is_same::value, - IsRowMajor = (IsDense&&MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 - : (IsDense&&MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 + IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 + : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 : XprTypeIsRowMajor, HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor), InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime), diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h index a036d8c3b..5dd3adeae 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h @@ -76,9 +76,7 @@ struct CommaInitializer template CommaInitializer& operator,(const DenseBase& other) { - if(other.cols()==0 || other.rows()==0) - return *this; - if (m_col==m_xpr.cols()) + if (m_col==m_xpr.cols() && (other.cols()!=0 || other.rows()!=m_currentBlockRows)) { m_row+=m_currentBlockRows; m_col = 0; @@ -86,24 +84,18 @@ struct CommaInitializer eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows() && "Too many rows passed to comma initializer (operator<<)"); } - eigen_assert(m_col - (m_row, m_col) = other; - else - m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other; + m_xpr.template block + (m_row, m_col, other.rows(), other.cols()) = other; m_col += other.cols(); return *this; } inline ~CommaInitializer() { - eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows() - && m_col == m_xpr.cols() - && "Too few coefficients passed to comma initializer (operator<<)"); + finished(); } /** \returns the built matrix once all its coefficients have been set. @@ -113,7 +105,12 @@ struct CommaInitializer * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished()); * \endcode */ - inline XprType& finished() { return m_xpr; } + inline XprType& finished() { + eigen_assert(((m_row+m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0) + && m_col == m_xpr.cols() + && "Too few coefficients passed to comma initializer (operator<<)"); + return m_xpr; + } XprType& m_xpr; // target expression Index m_row; // current row id diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h index e6c220f41..53c757bef 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h @@ -44,10 +44,10 @@ class DiagonalBase : public EigenBase template void evalTo(MatrixBase &other) const; template - void addTo(MatrixBase &other) const + inline void addTo(MatrixBase &other) const { other.diagonal() += diagonal(); } template - void subTo(MatrixBase &other) const + inline void subTo(MatrixBase &other) const { other.diagonal() -= diagonal(); } inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); } @@ -98,7 +98,7 @@ class DiagonalBase : public EigenBase template template -void DiagonalBase::evalTo(MatrixBase &other) const +inline void DiagonalBase::evalTo(MatrixBase &other) const { other.setZero(); other.diagonal() = diagonal(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h index 9d7651f1f..23aab831b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h @@ -59,7 +59,7 @@ struct dot_nocheck */ template template -typename internal::scalar_product_traits::Scalar,typename internal::traits::Scalar>::ReturnType +inline typename internal::scalar_product_traits::Scalar,typename internal::traits::Scalar>::ReturnType MatrixBase::dot(const MatrixBase& other) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h index 5f14c6587..5a1b2f28a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h @@ -969,6 +969,8 @@ template struct functor_traits > { enum { Cost = 1, PacketAccess = false }; }; +#if(__cplusplus < 201103L) +// std::binder* are deprecated since c++11 and will be removed in c++17 template struct functor_traits > { enum { Cost = functor_traits::Cost, PacketAccess = false }; }; @@ -976,6 +978,7 @@ struct functor_traits > template struct functor_traits > { enum { Cost = functor_traits::Cost, PacketAccess = false }; }; +#endif template struct functor_traits > diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index 29ac522d2..5744eb71e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h @@ -205,9 +205,6 @@ class GeneralProduct public: GeneralProduct(const Lhs& lhs, const Rhs& rhs) { - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - Base::coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum(); } @@ -264,8 +261,6 @@ class GeneralProduct GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) { - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) } struct set { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() = src; } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h index 5f783ebee..c6e93bbb0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h @@ -183,8 +183,8 @@ template inline void pstoreu(Scalar* to, const /** \internal tries to do cache prefetching of \a addr */ template inline void prefetch(const Scalar* addr) { -#if !defined(_MSC_VER) -__builtin_prefetch(addr); +#if (!EIGEN_COMP_MSVC) && (EIGEN_COMP_GNUC || EIGEN_COMP_CLANG || EIGEN_COMP_ICC) + __builtin_prefetch(addr); #endif } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h index 4e17ecd4b..f707aa41e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h @@ -218,8 +218,8 @@ struct conj_retval * Implementation of abs2 * ****************************************************************************/ -template -struct abs2_impl +template +struct abs2_impl_default { typedef typename NumTraits::Real RealScalar; static inline RealScalar run(const Scalar& x) @@ -228,15 +228,26 @@ struct abs2_impl } }; -template -struct abs2_impl > +template +struct abs2_impl_default // IsComplex { - static inline RealScalar run(const std::complex& x) + typedef typename NumTraits::Real RealScalar; + static inline RealScalar run(const Scalar& x) { return real(x)*real(x) + imag(x)*imag(x); } }; +template +struct abs2_impl +{ + typedef typename NumTraits::Real RealScalar; + static inline RealScalar run(const Scalar& x) + { + return abs2_impl_default::IsComplex>::run(x); + } +}; + template struct abs2_retval { @@ -496,11 +507,24 @@ struct floor_log2 template struct random_default_impl { - typedef typename NumTraits::NonInteger NonInteger; - static inline Scalar run(const Scalar& x, const Scalar& y) { - return x + Scalar((NonInteger(y)-x+1) * std::rand() / (RAND_MAX + NonInteger(1))); + typedef typename conditional::IsSigned,std::ptrdiff_t,std::size_t>::type ScalarX; + if(y=x the result converted to an unsigned long is still correct. + std::size_t range = ScalarX(y)-ScalarX(x); + std::size_t offset = 0; + // rejection sampling + std::size_t divisor = 1; + std::size_t multiplier = 1; + if(range range); + return Scalar(ScalarX(x) + offset); } static inline Scalar run() diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 85ffae265..bda79fa04 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -584,10 +584,11 @@ struct permut_matrix_product_retval const Index n = Side==OnTheLeft ? rows() : cols(); // FIXME we need an is_same for expression that is not sensitive to constness. For instance // is_same_xpr, Block >::value should be true. + const typename Dest::Scalar *dst_data = internal::extract_data(dst); if( is_same::value && blas_traits::HasUsableDirectAccess && blas_traits::HasUsableDirectAccess - && extract_data(dst) == extract_data(m_matrix)) + && dst_data!=0 && dst_data == extract_data(m_matrix)) { // apply the permutation inplace Matrix mask(m_permutation.size()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index a4e4af4a7..9f71956ff 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -315,8 +315,8 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_STRONG_INLINE void resizeLike(const EigenBase& _other) { const OtherDerived& other = _other.derived(); - internal::check_rows_cols_for_overflow::run(other.rows(), other.cols()); - const Index othersize = other.rows()*other.cols(); + internal::check_rows_cols_for_overflow::run(Index(other.rows()), Index(other.cols())); + const Index othersize = Index(other.rows())*Index(other.cols()); if(RowsAtCompileTime == 1) { eigen_assert(other.rows() == 1 || other.cols() == 1); @@ -487,7 +487,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type /** \sa MatrixBase::operator=(const EigenBase&) */ template EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase &other) - : m_storage(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) + : m_storage(Index(other.derived().rows()) * Index(other.derived().cols()), other.derived().rows(), other.derived().cols()) { _check_template_params(); internal::check_rows_cols_for_overflow::run(other.derived().rows(), other.derived().cols()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h index e30ae3d28..041f8222a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h @@ -76,9 +76,23 @@ template class Reverse EIGEN_DENSE_PUBLIC_INTERFACE(Reverse) using Base::IsRowMajor; - // next line is necessary because otherwise const version of operator() - // is hidden by non-const version defined in this file - using Base::operator(); + // The following two operators are provided to worarkound + // a MSVC 2013 issue. In theory, we could simply do: + // using Base::operator(); + // to make const version of operator() visible. + // Otheriwse, they would be hidden by the non-const versions defined in this file + + inline CoeffReturnType operator()(Index row, Index col) const + { + eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); + return coeff(row, col); + } + + inline CoeffReturnType operator()(Index index) const + { + eigen_assert(index >= 0 && index < m_matrix.size()); + return coeff(index); + } protected: enum { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h index 83565ddd8..30c9c38ec 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h @@ -243,7 +243,8 @@ template struct triangular_solv template inline void evalTo(Dest& dst) const { - if(!(is_same::value && extract_data(dst) == extract_data(m_rhs))) + const typename Dest::Scalar *dst_data = internal::extract_data(dst); + if(!(is_same::value && dst_data!=0 && extract_data(dst) == extract_data(m_rhs))) dst = m_rhs; m_triangularMatrix.template solveInPlace(dst); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h index 22096ea2f..2abce3c66 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h @@ -331,11 +331,11 @@ inline void MatrixBase::adjointInPlace() namespace internal { -template -struct blas_traits > - : blas_traits +template +struct blas_traits > + : blas_traits::type> { - typedef SelfCwiseBinaryOp XprType; + typedef SelfCwiseBinaryOp XprType; static inline const XprType extract(const XprType& x) { return x; } }; @@ -392,7 +392,6 @@ struct checkTransposeAliasing_impl ::run(extract_data(dst), other)) && "aliasing detected during transposition, use transposeInPlace() " "or evaluate the rhs into a temporary using .eval()"); - } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h index e4ba0756f..16bc1ce15 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h @@ -376,7 +376,8 @@ struct transposition_matrix_product_retval const int size = m_transpositions.size(); Index j = 0; - if(!(is_same::value && extract_data(dst) == extract_data(m_matrix))) + const typename Dest::Scalar *dst_data = internal::extract_data(dst); + if(!(is_same::value && dst_data!=0 && dst_data == extract_data(m_matrix))) dst = m_matrix; for(int k=(Transposed?size-1:0) ; Transposed?k>=0:k0 ? l2/(4 * sizeof(Scalar) * otherStride) : 0; + Index subcols = cols>0 ? l2/(4 * sizeof(Scalar) * std::max(otherStride,size)) : 0; subcols = std::max((subcols/Traits::nr)*Traits::nr, Traits::nr); for(Index k2=IsLower ? 0 : size; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h index a28f16fa0..e74381432 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h @@ -42,16 +42,29 @@ template struct conj_if; template<> struct conj_if { template - inline T operator()(const T& x) { return numext::conj(x); } + inline T operator()(const T& x) const { return numext::conj(x); } template - inline T pconj(const T& x) { return internal::pconj(x); } + inline T pconj(const T& x) const { return internal::pconj(x); } }; template<> struct conj_if { template - inline const T& operator()(const T& x) { return x; } + inline const T& operator()(const T& x) const { return x; } template - inline const T& pconj(const T& x) { return x; } + inline const T& pconj(const T& x) const { return x; } +}; + +// Generic implementation for custom complex types. +template +struct conj_helper +{ + typedef typename scalar_product_traits::ReturnType Scalar; + + EIGEN_STRONG_INLINE Scalar pmadd(const LhsScalar& x, const RhsScalar& y, const Scalar& c) const + { return padd(c, pmul(x,y)); } + + EIGEN_STRONG_INLINE Scalar pmul(const LhsScalar& x, const RhsScalar& y) const + { return conj_if()(x) * conj_if()(y); } }; template struct conj_helper @@ -171,12 +184,13 @@ template struct blas_traits }; // pop conjugate -template -struct blas_traits, NestedXpr> > - : blas_traits +template +struct blas_traits, Xpr> > + : blas_traits::type> { + typedef typename internal::remove_all::type NestedXpr; typedef blas_traits Base; - typedef CwiseUnaryOp, NestedXpr> XprType; + typedef CwiseUnaryOp, Xpr> XprType; typedef typename Base::ExtractType ExtractType; enum { @@ -188,12 +202,13 @@ struct blas_traits, NestedXpr> > }; // pop scalar multiple -template -struct blas_traits, NestedXpr> > - : blas_traits +template +struct blas_traits, Xpr> > + : blas_traits::type> { + typedef typename internal::remove_all::type NestedXpr; typedef blas_traits Base; - typedef CwiseUnaryOp, NestedXpr> XprType; + typedef CwiseUnaryOp, Xpr> XprType; typedef typename Base::ExtractType ExtractType; static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); } static inline Scalar extractScalarFactor(const XprType& x) @@ -201,12 +216,13 @@ struct blas_traits, NestedXpr> > }; // pop opposite -template -struct blas_traits, NestedXpr> > - : blas_traits +template +struct blas_traits, Xpr> > + : blas_traits::type> { + typedef typename internal::remove_all::type NestedXpr; typedef blas_traits Base; - typedef CwiseUnaryOp, NestedXpr> XprType; + typedef CwiseUnaryOp, Xpr> XprType; typedef typename Base::ExtractType ExtractType; static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); } static inline Scalar extractScalarFactor(const XprType& x) @@ -214,13 +230,14 @@ struct blas_traits, NestedXpr> > }; // pop/push transpose -template -struct blas_traits > - : blas_traits +template +struct blas_traits > + : blas_traits::type> { + typedef typename internal::remove_all::type NestedXpr; typedef typename NestedXpr::Scalar Scalar; typedef blas_traits Base; - typedef Transpose XprType; + typedef Transpose XprType; typedef Transpose ExtractType; // const to get rid of a compile error; anyway blas traits are only used on the RHS typedef Transpose _ExtractType; typedef typename conditional=6 + + #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS + #pragma GCC diagnostic push + #endif + #pragma GCC diagnostic ignored "-Wignored-attributes" + #endif #endif // not EIGEN_WARNINGS_DISABLED diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index e0d90eb82..7d9c89fa1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h @@ -13,7 +13,7 @@ #define EIGEN_WORLD_VERSION 3 #define EIGEN_MAJOR_VERSION 2 -#define EIGEN_MINOR_VERSION 8 +#define EIGEN_MINOR_VERSION 10 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index bc1ea69ed..ffa7e34f3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -507,7 +507,12 @@ template void smart_copy(const T* start, const T* end, T* target) template struct smart_copy_helper { static inline void run(const T* start, const T* end, T* target) - { memcpy(target, start, std::ptrdiff_t(end)-std::ptrdiff_t(start)); } + { + std::ptrdiff_t size = std::ptrdiff_t(end)-std::ptrdiff_t(start); + if(size==0) return; + eigen_internal_assert(start!=0 && end!=0 && target!=0); + memcpy(target, start, size); + } }; template struct smart_copy_helper { @@ -515,7 +520,6 @@ template struct smart_copy_helper { { std::copy(start, end, target); } }; - /***************************************************************************** *** Implementation of runtime stack allocation (falling back to malloc) *** *****************************************************************************/ @@ -655,99 +659,60 @@ template class aligned_stack_memory_handler /****************************************************************************/ + /** \class aligned_allocator -* \ingroup Core_Module -* -* \brief STL compatible allocator to use with with 16 byte aligned types -* -* Example: -* \code -* // Matrix4f requires 16 bytes alignment: -* std::map< int, Matrix4f, std::less, -* aligned_allocator > > my_map_mat4; -* // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator: -* std::map< int, Vector3f > my_map_vec3; -* \endcode -* -* \sa \ref TopicStlContainers. -*/ + * \ingroup Core_Module + * + * \brief STL compatible allocator to use with with 16 byte aligned types + * + * Example: + * \code + * // Matrix4f requires 16 bytes alignment: + * std::map< int, Matrix4f, std::less, + * aligned_allocator > > my_map_mat4; + * // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator: + * std::map< int, Vector3f > my_map_vec3; + * \endcode + * + * \sa \blank \ref TopicStlContainers. + */ template -class aligned_allocator +class aligned_allocator : public std::allocator { public: - typedef size_t size_type; - typedef std::ptrdiff_t difference_type; - typedef T* pointer; - typedef const T* const_pointer; - typedef T& reference; - typedef const T& const_reference; - typedef T value_type; + typedef size_t size_type; + typedef std::ptrdiff_t difference_type; + typedef T* pointer; + typedef const T* const_pointer; + typedef T& reference; + typedef const T& const_reference; + typedef T value_type; - template - struct rebind - { - typedef aligned_allocator other; - }; + template + struct rebind + { + typedef aligned_allocator other; + }; - pointer address( reference value ) const - { - return &value; - } + aligned_allocator() : std::allocator() {} - const_pointer address( const_reference value ) const - { - return &value; - } + aligned_allocator(const aligned_allocator& other) : std::allocator(other) {} - aligned_allocator() - { - } + template + aligned_allocator(const aligned_allocator& other) : std::allocator(other) {} - aligned_allocator( const aligned_allocator& ) - { - } + ~aligned_allocator() {} - template - aligned_allocator( const aligned_allocator& ) - { - } + pointer allocate(size_type num, const void* /*hint*/ = 0) + { + internal::check_size_for_overflow(num); + return static_cast( internal::aligned_malloc(num * sizeof(T)) ); + } - ~aligned_allocator() - { - } - - size_type max_size() const - { - return (std::numeric_limits::max)(); - } - - pointer allocate( size_type num, const void* hint = 0 ) - { - EIGEN_UNUSED_VARIABLE(hint); - internal::check_size_for_overflow(num); - return static_cast( internal::aligned_malloc( num * sizeof(T) ) ); - } - - void construct( pointer p, const T& value ) - { - ::new( p ) T( value ); - } - - void destroy( pointer p ) - { - p->~T(); - } - - void deallocate( pointer p, size_type /*num*/ ) - { - internal::aligned_free( p ); - } - - bool operator!=(const aligned_allocator& ) const - { return false; } - - bool operator==(const aligned_allocator& ) const - { return true; } + void deallocate(pointer p, size_type /*num*/) + { + internal::aligned_free(p); + } }; //---------- Cache sizes ---------- diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h index 5ddfbd4aa..d573bbd4a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h @@ -8,7 +8,10 @@ #pragma warning pop #elif defined __clang__ #pragma clang diagnostic pop + #elif defined __GNUC__ && __GNUC__>=6 + #pragma GCC diagnostic pop #endif + #endif #endif // EIGEN_WARNINGS_DISABLED diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h index 956e80d9e..e5131d20b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h @@ -327,13 +327,33 @@ GeneralizedEigenSolver::compute(const MatrixType& A, const MatrixTyp } else { - Scalar p = Scalar(0.5) * (m_matS.coeff(i, i) - m_matS.coeff(i+1, i+1)); - Scalar z = sqrt(abs(p * p + m_matS.coeff(i+1, i) * m_matS.coeff(i, i+1))); - m_alphas.coeffRef(i) = ComplexScalar(m_matS.coeff(i+1, i+1) + p, z); - m_alphas.coeffRef(i+1) = ComplexScalar(m_matS.coeff(i+1, i+1) + p, -z); + // We need to extract the generalized eigenvalues of the pair of a general 2x2 block S and a triangular 2x2 block T + // From the eigen decomposition of T = U * E * U^-1, + // we can extract the eigenvalues of (U^-1 * S * U) / E + // Here, we can take advantage that E = diag(T), and U = [ 1 T_01 ; 0 T_11-T_00], and U^-1 = [1 -T_11/(T_11-T_00) ; 0 1/(T_11-T_00)]. + // Then taking beta=T_00*T_11*(T_11-T_00), we can avoid any division, and alpha is the eigenvalues of A = (U^-1 * S * U) * diag(T_11,T_00) * (T_11-T_00): + + // T = [a b ; 0 c] + // S = [e f ; g h] + RealScalar a = m_realQZ.matrixT().coeff(i, i), b = m_realQZ.matrixT().coeff(i, i+1), c = m_realQZ.matrixT().coeff(i+1, i+1); + RealScalar e = m_matS.coeff(i, i), f = m_matS.coeff(i, i+1), g = m_matS.coeff(i+1, i), h = m_matS.coeff(i+1, i+1); + RealScalar d = c-a; + RealScalar gb = g*b; + Matrix A; + A << (e*d-gb)*c, ((e*b+f*d-h*b)*d-gb*b)*a, + g*c , (gb+h*d)*a; + + // NOTE, we could also compute the SVD of T's block during the QZ factorization so that the respective T block is guaranteed to be diagonal, + // and then we could directly apply the formula below (while taking care of scaling S columns by T11,T00): + + Scalar p = Scalar(0.5) * (A.coeff(i, i) - A.coeff(i+1, i+1)); + Scalar z = sqrt(abs(p * p + A.coeff(i+1, i) * A.coeff(i, i+1))); + m_alphas.coeffRef(i) = ComplexScalar(A.coeff(i+1, i+1) + p, z); + m_alphas.coeffRef(i+1) = ComplexScalar(A.coeff(i+1, i+1) + p, -z); + + m_betas.coeffRef(i) = + m_betas.coeffRef(i+1) = a*c*d; - m_betas.coeffRef(i) = m_realQZ.matrixT().coeff(i,i); - m_betas.coeffRef(i+1) = m_realQZ.matrixT().coeff(i,i); i += 2; } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h index 192278d68..a63c08aee 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h @@ -367,10 +367,10 @@ void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs) hCoeffs.tail(n-i-1).noalias() = (matA.bottomRightCorner(remainingSize,remainingSize).template selfadjointView() * (conj(h) * matA.col(i).tail(remainingSize))); - hCoeffs.tail(n-i-1) += (conj(h)*Scalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1); + hCoeffs.tail(n-i-1) += (conj(h)*RealScalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1); matA.bottomRightCorner(remainingSize, remainingSize).template selfadjointView() - .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), -1); + .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), Scalar(-1)); matA.col(i).coeffRef(i+1) = beta; hCoeffs.coeffRef(i) = h; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h index 372e422b9..820ac96fe 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h @@ -75,7 +75,7 @@ template class Homogeneous inline Index rows() const { return m_matrix.rows() + (int(Direction)==Vertical ? 1 : 0); } inline Index cols() const { return m_matrix.cols() + (int(Direction)==Horizontal ? 1 : 0); } - inline Scalar coeff(Index row, Index col) const + inline Scalar coeff(Index row, Index col=0) const { if( (int(Direction)==Vertical && row==m_matrix.rows()) || (int(Direction)==Horizontal && col==m_matrix.cols())) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index 25ed17bb6..e89ba80b2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -276,7 +276,7 @@ public: inline Coefficients& coeffs() { return m_coeffs;} inline const Coefficients& coeffs() const { return m_coeffs;} - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(IsAligned)) protected: Coefficients m_coeffs; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 0186f3b82..e87eec201 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -443,7 +443,7 @@ public: operator * (const DiagonalBase &b) const { TransformTimeDiagonalReturnType res(*this); - res.linear() *= b; + res.linearExt() *= b; return res; } @@ -557,7 +557,7 @@ public: return res; } - inline Transform& operator*=(const DiagonalMatrix& s) { linear() *= s; return *this; } + inline Transform& operator*=(const DiagonalMatrix& s) { linearExt() *= s; return *this; } template inline Transform& operator=(const RotationBase& r); @@ -828,7 +828,7 @@ Transform::prescale(const MatrixBase &oth { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) - m_matrix.template block(0,0).noalias() = (other.asDiagonal() * m_matrix.template block(0,0)); + affine().noalias() = (other.asDiagonal() * affine()); return *this; } @@ -1048,7 +1048,7 @@ void Transform::computeRotationScaling(RotationMatrixTy } } -/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being +/** decomposes the linear part of the transformation as a product scaling x rotation, the scaling being * not necessarily positive. * * If either pointer is zero, the corresponding computation is skipped. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h index 2e7798686..84f6a0d12 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h @@ -130,8 +130,10 @@ public: } /** Applies translation to vector */ - inline VectorType operator* (const VectorType& other) const - { return m_coeffs + other; } + template + inline typename internal::enable_if::type + operator* (const MatrixBase& vec) const + { return m_coeffs + vec.derived(); } /** \returns the inverse translation (opposite) */ Translation inverse() const { return Translation(-m_coeffs); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h b/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h index 32112af9b..4c1f499a1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h @@ -75,8 +75,9 @@ void MatrixBase::makeHouseholder( RealScalar tailSqNorm = size()==1 ? RealScalar(0) : tail.squaredNorm(); Scalar c0 = coeff(0); + const RealScalar tol = (std::numeric_limits::min)(); - if(tailSqNorm == RealScalar(0) && numext::imag(c0)==RealScalar(0)) + if(tailSqNorm <= tol && numext::abs2(numext::imag(c0))<=tol) { tau = RealScalar(0); beta = numext::real(c0); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h b/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h index d800ca1fa..aea2439a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h @@ -237,8 +237,9 @@ template class HouseholderS { workspace.resize(rows()); Index vecs = m_length; + const typename Dest::Scalar *dst_data = internal::extract_data(dst); if( internal::is_same::type,Dest>::value - && internal::extract_data(dst) == internal::extract_data(m_vectors)) + && dst_data!=0 && dst_data == internal::extract_data(m_vectors)) { // in-place dst.diagonal().setOnes(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h index 3cf887193..e836fd696 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h @@ -290,7 +290,7 @@ struct inverse_impl : public ReturnByValue > { const int Size = EIGEN_PLAIN_ENUM_MIN(MatrixType::ColsAtCompileTime,Dest::ColsAtCompileTime); EIGEN_ONLY_USED_FOR_DEBUG(Size); - eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=extract_data(dst))) + eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=0 && extract_data(m_matrix)!=extract_data(dst))) && "Aliasing problem detected in inverse(), you need to do inverse().eval() here."); compute_inverse::run(m_matrix, dst); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h index 60b7a2376..80ec674d6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h @@ -151,10 +151,12 @@ struct compute_inverse_size4 iC = _mm_mul_ps(rd,iC); iD = _mm_mul_ps(rd,iD); - result.template writePacket( 0, _mm_shuffle_ps(iA,iB,0x77)); - result.template writePacket( 4, _mm_shuffle_ps(iA,iB,0x22)); - result.template writePacket( 8, _mm_shuffle_ps(iC,iD,0x77)); - result.template writePacket(12, _mm_shuffle_ps(iC,iD,0x22)); + DenseIndex res_stride = result.outerStride(); + float* res = result.data(); + pstoret(res+0, _mm_shuffle_ps(iA,iB,0x77)); + pstoret(res+res_stride, _mm_shuffle_ps(iA,iB,0x22)); + pstoret(res+2*res_stride, _mm_shuffle_ps(iC,iD,0x77)); + pstoret(res+3*res_stride, _mm_shuffle_ps(iC,iD,0x22)); } }; @@ -311,14 +313,16 @@ struct compute_inverse_size4 iC1 = _mm_sub_pd(_mm_mul_pd(B1, dC), iC1); iC2 = _mm_sub_pd(_mm_mul_pd(B2, dC), iC2); - result.template writePacket( 0, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 3), d1)); // iA# / det - result.template writePacket( 4, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 0), d2)); - result.template writePacket( 2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 3), d1)); // iB# / det - result.template writePacket( 6, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 0), d2)); - result.template writePacket( 8, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 3), d1)); // iC# / det - result.template writePacket(12, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 0), d2)); - result.template writePacket(10, _mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 3), d1)); // iD# / det - result.template writePacket(14, _mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 0), d2)); + DenseIndex res_stride = result.outerStride(); + double* res = result.data(); + pstoret(res+0, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 3), d1)); + pstoret(res+res_stride, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 0), d2)); + pstoret(res+2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 3), d1)); + pstoret(res+res_stride+2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 0), d2)); + pstoret(res+2*res_stride, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 3), d1)); + pstoret(res+3*res_stride, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 0), d2)); + pstoret(res+2*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 3), d1)); + pstoret(res+3*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 0), d2)); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h index a955287d1..20acc0226 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h @@ -10,6 +10,14 @@ #ifndef EIGEN_PASTIXSUPPORT_H #define EIGEN_PASTIXSUPPORT_H +#if defined(DCOMPLEX) + #define PASTIX_COMPLEX COMPLEX + #define PASTIX_DCOMPLEX DCOMPLEX +#else + #define PASTIX_COMPLEX std::complex + #define PASTIX_DCOMPLEX std::complex +#endif + namespace Eigen { /** \ingroup PaStiXSupport_Module @@ -74,14 +82,14 @@ namespace internal { if (n == 0) { ptr = NULL; idx = NULL; vals = NULL; } if (nbrhs == 0) {x = NULL; nbrhs=1;} - c_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast(vals), perm, invp, reinterpret_cast(x), nbrhs, iparm, dparm); + c_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast(vals), perm, invp, reinterpret_cast(x), nbrhs, iparm, dparm); } void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, std::complex *vals, int *perm, int * invp, std::complex *x, int nbrhs, int *iparm, double *dparm) { if (n == 0) { ptr = NULL; idx = NULL; vals = NULL; } if (nbrhs == 0) {x = NULL; nbrhs=1;} - z_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast(vals), perm, invp, reinterpret_cast(x), nbrhs, iparm, dparm); + z_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast(vals), perm, invp, reinterpret_cast(x), nbrhs, iparm, dparm); } // Convert the matrix to Fortran-style Numbering diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h index 18cd7d88a..0faacc5f5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h @@ -221,11 +221,11 @@ class PardisoImpl m_type = type; bool symmetric = std::abs(m_type) < 10; m_iparm[0] = 1; // No solver default - m_iparm[1] = 3; // use Metis for the ordering - m_iparm[2] = 1; // Numbers of processors, value of OMP_NUM_THREADS + m_iparm[1] = 2; // use Metis for the ordering + m_iparm[2] = 0; // Reserved. Set to zero. (??Numbers of processors, value of OMP_NUM_THREADS??) m_iparm[3] = 0; // No iterative-direct algorithm m_iparm[4] = 0; // No user fill-in reducing permutation - m_iparm[5] = 0; // Write solution into x + m_iparm[5] = 0; // Write solution into x, b is left unchanged m_iparm[6] = 0; // Not in use m_iparm[7] = 2; // Max numbers of iterative refinement steps m_iparm[8] = 0; // Not in use @@ -246,7 +246,10 @@ class PardisoImpl m_iparm[26] = 0; // No matrix checker m_iparm[27] = (sizeof(RealScalar) == 4) ? 1 : 0; m_iparm[34] = 1; // C indexing - m_iparm[59] = 1; // Automatic switch between In-Core and Out-of-Core modes + m_iparm[36] = 0; // CSR + m_iparm[59] = 0; // 0 - In-Core ; 1 - Automatic switch between In-Core and Out-of-Core modes ; 2 - Out-of-Core + + memset(m_pt, 0, sizeof(m_pt)); } protected: @@ -384,7 +387,6 @@ bool PardisoImpl::_solve(const MatrixBase &b, MatrixBase::_solve(const MatrixBase &b, MatrixBase * * \sa \ref TutorialSparseDirectSolvers @@ -447,6 +452,9 @@ class PardisoLU : public PardisoImpl< PardisoLU > * using the Intel MKL PARDISO library. The sparse matrix A must be selfajoint and positive definite. * The vectors or matrices X and B can be either dense or sparse. * + * By default, it runs in in-core mode. To enable PARDISO's out-of-core feature, set: + * \code solver.pardisoParameterArray()[59] = 1; \endcode + * * \tparam MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> * \tparam UpLo can be any bitwise combination of Upper, Lower. The default is Upper, meaning only the upper triangular part has to be used. * Upper|Lower can be used to tell both triangular parts can be used as input. @@ -507,6 +515,9 @@ class PardisoLLT : public PardisoImpl< PardisoLLT > * For complex matrices, A can also be symmetric only, see the \a Options template parameter. * The vectors or matrices X and B can be either dense or sparse. * + * By default, it runs in in-core mode. To enable PARDISO's out-of-core feature, set: + * \code solver.pardisoParameterArray()[59] = 1; \endcode + * * \tparam MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> * \tparam Options can be any bitwise combination of Upper, Lower, and Symmetric. The default is Upper, meaning only the upper triangular part has to be used. * Symmetric can be used for symmetric, non-selfadjoint complex matrices, the default being to assume a selfadjoint matrix. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 567eab7cd..1ef77ae32 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -127,9 +127,6 @@ template class ColPivHouseholderQR * * \returns a solution. * - * \note The case where b is a matrix is not yet implemented. Also, this - * code is space inefficient. - * * \note_about_checking_solutions * * \note_about_arbitrary_choice_of_solution diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h index 0b39966e1..bdd994795 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h @@ -134,9 +134,6 @@ template class FullPivHouseholderQR * \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A, * and an arbitrary solution otherwise. * - * \note The case where b is a matrix is not yet implemented. Also, this - * code is space inefficient. - * * \note_about_checking_solutions * * \note_about_arbitrary_choice_of_solution diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h index 343a66499..e4a3a639f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h @@ -107,9 +107,6 @@ template class HouseholderQR * * \returns a solution. * - * \note The case where b is a matrix is not yet implemented. Also, this - * code is space inefficient. - * * \note_about_checking_solutions * * \note_about_arbitrary_choice_of_solution diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index 89ace381e..7a5821d4f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -359,29 +359,42 @@ struct svd_precondition_2x2_block_to_be_real SVD; typedef typename SVD::Index Index; - static void run(typename SVD::WorkMatrixType&, SVD&, Index, Index) {} + typedef typename MatrixType::RealScalar RealScalar; + static bool run(typename SVD::WorkMatrixType&, SVD&, Index, Index, RealScalar&) { return true; } }; template struct svd_precondition_2x2_block_to_be_real { typedef JacobiSVD SVD; + typedef typename SVD::Index Index; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; - typedef typename SVD::Index Index; - static void run(typename SVD::WorkMatrixType& work_matrix, SVD& svd, Index p, Index q) + static bool run(typename SVD::WorkMatrixType& work_matrix, SVD& svd, Index p, Index q, RealScalar& maxDiagEntry) { using std::sqrt; + using std::abs; + using std::max; Scalar z; JacobiRotation rot; RealScalar n = sqrt(numext::abs2(work_matrix.coeff(p,p)) + numext::abs2(work_matrix.coeff(q,p))); - + + const RealScalar considerAsZero = (std::numeric_limits::min)(); + const RealScalar precision = NumTraits::epsilon(); + if(n==0) { - z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); - work_matrix.row(p) *= z; - if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z); - if(work_matrix.coeff(q,q)!=Scalar(0)) + // make sure first column is zero + work_matrix.coeffRef(p,p) = work_matrix.coeffRef(q,p) = Scalar(0); + + if(abs(numext::imag(work_matrix.coeff(p,q)))>considerAsZero) + { + // work_matrix.coeff(p,q) can be zero if work_matrix.coeff(q,p) is not zero but small enough to underflow when computing n + z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); + work_matrix.row(p) *= z; + if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z); + } + if(abs(numext::imag(work_matrix.coeff(q,q)))>considerAsZero) { z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); work_matrix.row(q) *= z; @@ -395,19 +408,25 @@ struct svd_precondition_2x2_block_to_be_real rot.s() = work_matrix.coeff(q,p) / n; work_matrix.applyOnTheLeft(p,q,rot); if(svd.computeU()) svd.m_matrixU.applyOnTheRight(p,q,rot.adjoint()); - if(work_matrix.coeff(p,q) != Scalar(0)) + if(abs(numext::imag(work_matrix.coeff(p,q)))>considerAsZero) { - Scalar z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); + z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); work_matrix.col(q) *= z; if(svd.computeV()) svd.m_matrixV.col(q) *= z; } - if(work_matrix.coeff(q,q) != Scalar(0)) + if(abs(numext::imag(work_matrix.coeff(q,q)))>considerAsZero) { z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); work_matrix.row(q) *= z; if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z); } } + + // update largest diagonal entry + maxDiagEntry = max EIGEN_EMPTY (maxDiagEntry,max EIGEN_EMPTY (abs(work_matrix.coeff(p,p)), abs(work_matrix.coeff(q,q)))); + // and check whether the 2x2 block is already diagonal + RealScalar threshold = max EIGEN_EMPTY (considerAsZero, precision * maxDiagEntry); + return abs(work_matrix.coeff(p,q))>threshold || abs(work_matrix.coeff(q,p)) > threshold; } }; @@ -424,22 +443,23 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, JacobiRotation rot1; RealScalar t = m.coeff(0,0) + m.coeff(1,1); RealScalar d = m.coeff(1,0) - m.coeff(0,1); - if(t == RealScalar(0)) + if(d == RealScalar(0)) { - rot1.c() = RealScalar(0); - rot1.s() = d > RealScalar(0) ? RealScalar(1) : RealScalar(-1); + rot1.s() = RealScalar(0); + rot1.c() = RealScalar(1); } else { - RealScalar t2d2 = numext::hypot(t,d); - rot1.c() = abs(t)/t2d2; - rot1.s() = d/t2d2; - if(tmakeJacobi(m,0,1); - *j_left = rot1 * j_right->transpose(); + *j_left = rot1 * j_right->transpose(); } } // end namespace internal @@ -826,6 +846,7 @@ JacobiSVD::compute(const MatrixType& matrix, unsig check_template_parameters(); using std::abs; + using std::max; allocate(matrix.rows(), matrix.cols(), computationOptions); // currently we stop when we reach precision 2*epsilon as the last bit of precision can require an unreasonable number of iterations, @@ -857,6 +878,7 @@ JacobiSVD::compute(const MatrixType& matrix, unsig } /*** step 2. The main Jacobi SVD iteration. ***/ + RealScalar maxDiagEntry = m_workMatrix.cwiseAbs().diagonal().maxCoeff(); bool finished = false; while(!finished) @@ -872,25 +894,27 @@ JacobiSVD::compute(const MatrixType& matrix, unsig // if this 2x2 sub-matrix is not diagonal already... // notice that this comparison will evaluate to false if any NaN is involved, ensuring that NaN's don't // keep us iterating forever. Similarly, small denormal numbers are considered zero. - using std::max; - RealScalar threshold = (max)(considerAsZero, precision * (max)(abs(m_workMatrix.coeff(p,p)), - abs(m_workMatrix.coeff(q,q)))); - // We compare both values to threshold instead of calling max to be robust to NaN (See bug 791) + RealScalar threshold = max EIGEN_EMPTY (considerAsZero, precision * maxDiagEntry); if(abs(m_workMatrix.coeff(p,q))>threshold || abs(m_workMatrix.coeff(q,p)) > threshold) { finished = false; - // perform SVD decomposition of 2x2 sub-matrix corresponding to indices p,q to make it diagonal - internal::svd_precondition_2x2_block_to_be_real::run(m_workMatrix, *this, p, q); - JacobiRotation j_left, j_right; - internal::real_2x2_jacobi_svd(m_workMatrix, p, q, &j_left, &j_right); + // the complex to real operation returns true is the updated 2x2 block is not already diagonal + if(internal::svd_precondition_2x2_block_to_be_real::run(m_workMatrix, *this, p, q, maxDiagEntry)) + { + JacobiRotation j_left, j_right; + internal::real_2x2_jacobi_svd(m_workMatrix, p, q, &j_left, &j_right); - // accumulate resulting Jacobi rotations - m_workMatrix.applyOnTheLeft(p,q,j_left); - if(computeU()) m_matrixU.applyOnTheRight(p,q,j_left.transpose()); + // accumulate resulting Jacobi rotations + m_workMatrix.applyOnTheLeft(p,q,j_left); + if(computeU()) m_matrixU.applyOnTheRight(p,q,j_left.transpose()); - m_workMatrix.applyOnTheRight(p,q,j_right); - if(computeV()) m_matrixV.applyOnTheRight(p,q,j_right); + m_workMatrix.applyOnTheRight(p,q,j_right); + if(computeV()) m_matrixV.applyOnTheRight(p,q,j_right); + + // keep track of the largest diagonal coefficient + maxDiagEntry = max EIGEN_EMPTY (maxDiagEntry,max EIGEN_EMPTY (abs(m_workMatrix.coeff(p,p)), abs(m_workMatrix.coeff(q,q)))); + } } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h index a667cb56e..34cad3df7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h @@ -102,6 +102,11 @@ class CompressedStorage inline size_t allocatedSize() const { return m_allocatedSize; } inline void clear() { m_size = 0; } + const Scalar* valuePtr() const { return m_values; } + Scalar* valuePtr() { return m_values; } + const Index* indexPtr() const { return m_indices; } + Index* indexPtr() { return m_indices; } + inline Scalar& value(size_t i) { return m_values[i]; } inline const Scalar& value(size_t i) const { return m_values[i]; } @@ -208,8 +213,10 @@ class CompressedStorage Index* newIndices = new Index[size]; size_t copySize = (std::min)(size, m_size); // copy - internal::smart_copy(m_values, m_values+copySize, newValues); - internal::smart_copy(m_indices, m_indices+copySize, newIndices); + if (copySize>0) { + internal::smart_copy(m_values, m_values+copySize, newValues); + internal::smart_copy(m_indices, m_indices+copySize, newIndices); + } // delete old stuff delete[] m_values; delete[] m_indices; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 4f4983508..99886079d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -1,539 +1,623 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008-2009 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_SPARSE_BLOCK_H -#define EIGEN_SPARSE_BLOCK_H - -namespace Eigen { - -template -class BlockImpl - : public SparseMatrixBase > -{ - typedef typename internal::remove_all::type _MatrixTypeNested; - typedef Block BlockType; -public: - enum { IsRowMajor = internal::traits::IsRowMajor }; -protected: - enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; -public: - EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) - - class InnerIterator: public XprType::InnerIterator - { - typedef typename BlockImpl::Index Index; - public: - inline InnerIterator(const BlockType& xpr, Index outer) - : XprType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - class ReverseInnerIterator: public XprType::ReverseInnerIterator - { - typedef typename BlockImpl::Index Index; - public: - inline ReverseInnerIterator(const BlockType& xpr, Index outer) - : XprType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - - inline BlockImpl(const XprType& xpr, int i) - : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) - {} - - inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) - : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) - {} - - inline const Scalar coeff(int row, int col) const - { - return m_matrix.coeff(row + IsRowMajor ? m_outerStart : 0, col +IsRowMajor ? 0 : m_outerStart); - } - - inline const Scalar coeff(int index) const - { - return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); - } - - EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } - EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } - - protected: - - typename XprType::Nested m_matrix; - Index m_outerStart; - const internal::variable_if_dynamic m_outerSize; - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) - private: - Index nonZeros() const; -}; - - -/*************************************************************************** -* specialisation for SparseMatrix -***************************************************************************/ - -template -class BlockImpl,BlockRows,BlockCols,true,Sparse> - : public SparseMatrixBase,BlockRows,BlockCols,true> > -{ - typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; - typedef typename internal::remove_all::type _MatrixTypeNested; - typedef Block BlockType; - typedef Block ConstBlockType; -public: - enum { IsRowMajor = internal::traits::IsRowMajor }; - EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) -protected: - enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; -public: - - class InnerIterator: public SparseMatrixType::InnerIterator - { - public: - inline InnerIterator(const BlockType& xpr, Index outer) - : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator - { - public: - inline ReverseInnerIterator(const BlockType& xpr, Index outer) - : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - - inline BlockImpl(const SparseMatrixType& xpr, int i) - : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) - {} - - inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) - : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) - {} - - template - inline BlockType& operator=(const SparseMatrixBase& other) - { - typedef typename internal::remove_all::type _NestedMatrixType; - _NestedMatrixType& matrix = const_cast<_NestedMatrixType&>(m_matrix);; - // This assignement is slow if this vector set is not empty - // and/or it is not at the end of the nonzeros of the underlying matrix. - - // 1 - eval to a temporary to avoid transposition and/or aliasing issues - SparseMatrix tmp(other); - - // 2 - let's check whether there is enough allocated memory - Index nnz = tmp.nonZeros(); - Index start = m_outerStart==0 ? 0 : matrix.outerIndexPtr()[m_outerStart]; // starting position of the current block - Index end = m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]; // ending posiiton of the current block - Index block_size = end - start; // available room in the current block - Index tail_size = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end; - - Index free_size = m_matrix.isCompressed() - ? Index(matrix.data().allocatedSize()) + block_size - : block_size; - - if(nnz>free_size) - { - // realloc manually to reduce copies - typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz); - - std::memcpy(&newdata.value(0), &m_matrix.data().value(0), start*sizeof(Scalar)); - std::memcpy(&newdata.index(0), &m_matrix.data().index(0), start*sizeof(Index)); - - std::memcpy(&newdata.value(start), &tmp.data().value(0), nnz*sizeof(Scalar)); - std::memcpy(&newdata.index(start), &tmp.data().index(0), nnz*sizeof(Index)); - - std::memcpy(&newdata.value(start+nnz), &matrix.data().value(end), tail_size*sizeof(Scalar)); - std::memcpy(&newdata.index(start+nnz), &matrix.data().index(end), tail_size*sizeof(Index)); - - newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz); - - matrix.data().swap(newdata); - } - else - { - // no need to realloc, simply copy the tail at its respective position and insert tmp - matrix.data().resize(start + nnz + tail_size); - - std::memmove(&matrix.data().value(start+nnz), &matrix.data().value(end), tail_size*sizeof(Scalar)); - std::memmove(&matrix.data().index(start+nnz), &matrix.data().index(end), tail_size*sizeof(Index)); - - std::memcpy(&matrix.data().value(start), &tmp.data().value(0), nnz*sizeof(Scalar)); - std::memcpy(&matrix.data().index(start), &tmp.data().index(0), nnz*sizeof(Index)); - } - - // update innerNonZeros - if(!m_matrix.isCompressed()) - for(Index j=0; j(other); - } - - inline const Scalar* valuePtr() const - { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - inline Scalar* valuePtr() - { return m_matrix.const_cast_derived().valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - - inline const Index* innerIndexPtr() const - { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - inline Index* innerIndexPtr() - { return m_matrix.const_cast_derived().innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - - inline const Index* outerIndexPtr() const - { return m_matrix.outerIndexPtr() + m_outerStart; } - inline Index* outerIndexPtr() - { return m_matrix.const_cast_derived().outerIndexPtr() + m_outerStart; } - - Index nonZeros() const - { - if(m_matrix.isCompressed()) - return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) - - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); - else if(m_outerSize.value()==0) - return 0; - else - return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); - } - - inline Scalar& coeffRef(int row, int col) - { - return m_matrix.const_cast_derived().coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); - } - - inline const Scalar coeff(int row, int col) const - { - return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); - } - - inline const Scalar coeff(int index) const - { - return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); - } - - const Scalar& lastCoeff() const - { - EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); - eigen_assert(nonZeros()>0); - if(m_matrix.isCompressed()) - return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; - else - return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; - } - - EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } - EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } - - protected: - - typename SparseMatrixType::Nested m_matrix; - Index m_outerStart; - const internal::variable_if_dynamic m_outerSize; - -}; - - -template -class BlockImpl,BlockRows,BlockCols,true,Sparse> - : public SparseMatrixBase,BlockRows,BlockCols,true> > -{ - typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; - typedef typename internal::remove_all::type _MatrixTypeNested; - typedef Block BlockType; -public: - enum { IsRowMajor = internal::traits::IsRowMajor }; - EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) -protected: - enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; -public: - - class InnerIterator: public SparseMatrixType::InnerIterator - { - public: - inline InnerIterator(const BlockType& xpr, Index outer) - : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator - { - public: - inline ReverseInnerIterator(const BlockType& xpr, Index outer) - : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - - inline BlockImpl(const SparseMatrixType& xpr, int i) - : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) - {} - - inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) - : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) - {} - - inline const Scalar* valuePtr() const - { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - - inline const Index* innerIndexPtr() const - { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - - inline const Index* outerIndexPtr() const - { return m_matrix.outerIndexPtr() + m_outerStart; } - - Index nonZeros() const - { - if(m_matrix.isCompressed()) - return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) - - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); - else if(m_outerSize.value()==0) - return 0; - else - return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); - } - - inline const Scalar coeff(int row, int col) const - { - return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); - } - - inline const Scalar coeff(int index) const - { - return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); - } - - const Scalar& lastCoeff() const - { - EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); - eigen_assert(nonZeros()>0); - if(m_matrix.isCompressed()) - return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; - else - return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; - } - - EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } - EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } - - protected: - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) - - typename SparseMatrixType::Nested m_matrix; - Index m_outerStart; - const internal::variable_if_dynamic m_outerSize; -}; - -//---------- - -/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this - * is col-major (resp. row-major). - */ -template -typename SparseMatrixBase::InnerVectorReturnType SparseMatrixBase::innerVector(Index outer) -{ return InnerVectorReturnType(derived(), outer); } - -/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this - * is col-major (resp. row-major). Read-only. - */ -template -const typename SparseMatrixBase::ConstInnerVectorReturnType SparseMatrixBase::innerVector(Index outer) const -{ return ConstInnerVectorReturnType(derived(), outer); } - -/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this - * is col-major (resp. row-major). - */ -template -typename SparseMatrixBase::InnerVectorsReturnType -SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) -{ - return Block(derived(), - IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, - IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize); - -} - -/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this - * is col-major (resp. row-major). Read-only. - */ -template -const typename SparseMatrixBase::ConstInnerVectorsReturnType -SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) const -{ - return Block(derived(), - IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, - IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize); - -} - -/** Generic implementation of sparse Block expression. - * Real-only. - */ -template -class BlockImpl - : public SparseMatrixBase >, internal::no_assignment_operator -{ - typedef typename internal::remove_all::type _MatrixTypeNested; - typedef Block BlockType; -public: - enum { IsRowMajor = internal::traits::IsRowMajor }; - EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) - - /** Column or Row constructor - */ - inline BlockImpl(const XprType& xpr, int i) - : m_matrix(xpr), - m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0), - m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0), - m_blockRows(BlockRows==1 ? 1 : xpr.rows()), - m_blockCols(BlockCols==1 ? 1 : xpr.cols()) - {} - - /** Dynamic-size constructor - */ - inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) - : m_matrix(xpr), m_startRow(startRow), m_startCol(startCol), m_blockRows(blockRows), m_blockCols(blockCols) - {} - - inline int rows() const { return m_blockRows.value(); } - inline int cols() const { return m_blockCols.value(); } - - inline Scalar& coeffRef(int row, int col) - { - return m_matrix.const_cast_derived() - .coeffRef(row + m_startRow.value(), col + m_startCol.value()); - } - - inline const Scalar coeff(int row, int col) const - { - return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value()); - } - - inline Scalar& coeffRef(int index) - { - return m_matrix.const_cast_derived() - .coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), - m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); - } - - inline const Scalar coeff(int index) const - { - return m_matrix - .coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), - m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); - } - - inline const _MatrixTypeNested& nestedExpression() const { return m_matrix; } - - class InnerIterator : public _MatrixTypeNested::InnerIterator - { - typedef typename _MatrixTypeNested::InnerIterator Base; - const BlockType& m_block; - Index m_end; - public: - - EIGEN_STRONG_INLINE InnerIterator(const BlockType& block, Index outer) - : Base(block.derived().nestedExpression(), outer + (IsRowMajor ? block.m_startRow.value() : block.m_startCol.value())), - m_block(block), - m_end(IsRowMajor ? block.m_startCol.value()+block.m_blockCols.value() : block.m_startRow.value()+block.m_blockRows.value()) - { - while( (Base::operator bool()) && (Base::index() < (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value())) ) - Base::operator++(); - } - - inline Index index() const { return Base::index() - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } - inline Index outer() const { return Base::outer() - (IsRowMajor ? m_block.m_startRow.value() : m_block.m_startCol.value()); } - inline Index row() const { return Base::row() - m_block.m_startRow.value(); } - inline Index col() const { return Base::col() - m_block.m_startCol.value(); } - - inline operator bool() const { return Base::operator bool() && Base::index() < m_end; } - }; - class ReverseInnerIterator : public _MatrixTypeNested::ReverseInnerIterator - { - typedef typename _MatrixTypeNested::ReverseInnerIterator Base; - const BlockType& m_block; - Index m_begin; - public: - - EIGEN_STRONG_INLINE ReverseInnerIterator(const BlockType& block, Index outer) - : Base(block.derived().nestedExpression(), outer + (IsRowMajor ? block.m_startRow.value() : block.m_startCol.value())), - m_block(block), - m_begin(IsRowMajor ? block.m_startCol.value() : block.m_startRow.value()) - { - while( (Base::operator bool()) && (Base::index() >= (IsRowMajor ? m_block.m_startCol.value()+block.m_blockCols.value() : m_block.m_startRow.value()+block.m_blockRows.value())) ) - Base::operator--(); - } - - inline Index index() const { return Base::index() - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } - inline Index outer() const { return Base::outer() - (IsRowMajor ? m_block.m_startRow.value() : m_block.m_startCol.value()); } - inline Index row() const { return Base::row() - m_block.m_startRow.value(); } - inline Index col() const { return Base::col() - m_block.m_startCol.value(); } - - inline operator bool() const { return Base::operator bool() && Base::index() >= m_begin; } - }; - protected: - friend class InnerIterator; - friend class ReverseInnerIterator; - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) - - typename XprType::Nested m_matrix; - const internal::variable_if_dynamic m_startRow; - const internal::variable_if_dynamic m_startCol; - const internal::variable_if_dynamic m_blockRows; - const internal::variable_if_dynamic m_blockCols; - private: - Index nonZeros() const; -}; - -} // end namespace Eigen - -#endif // EIGEN_SPARSE_BLOCK_H - +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_SPARSE_BLOCK_H +#define EIGEN_SPARSE_BLOCK_H + +namespace Eigen { + +template +class BlockImpl + : public SparseMatrixBase > +{ +public: + typedef Block BlockType; + enum { IsRowMajor = internal::traits::IsRowMajor }; +protected: + typedef typename internal::remove_all::type _MatrixTypeNested; + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) + + class InnerIterator: public XprType::InnerIterator + { + typedef typename BlockImpl::Index Index; + public: + inline InnerIterator(const Block& xpr, Index outer) + : XprType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public XprType::ReverseInnerIterator + { + typedef typename BlockImpl::Index Index; + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : XprType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const XprType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + IsRowMajor ? m_outerStart : 0, col +IsRowMajor ? 0 : m_outerStart); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + typename XprType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + private: + Index nonZeros() const; +}; + + +/*************************************************************************** +* specialisation for SparseMatrix +***************************************************************************/ + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public SparseMatrixBase,BlockRows,BlockCols,true> > +{ + typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef Block ConstBlockType; +public: + typedef Block BlockType; + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) +protected: + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + + class InnerIterator: public SparseMatrixType::InnerIterator + { + public: + inline InnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator + { + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const SparseMatrixType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + template + inline BlockType& operator=(const SparseMatrixBase& other) + { + typedef typename internal::remove_all::type _NestedMatrixType; + _NestedMatrixType& matrix = const_cast<_NestedMatrixType&>(m_matrix);; + // This assignement is slow if this vector set is not empty + // and/or it is not at the end of the nonzeros of the underlying matrix. + + // 1 - eval to a temporary to avoid transposition and/or aliasing issues + SparseMatrix tmp(other); + + // 2 - let's check whether there is enough allocated memory + Index nnz = tmp.nonZeros(); + Index start = m_outerStart==0 ? 0 : matrix.outerIndexPtr()[m_outerStart]; // starting position of the current block + Index end = m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]; // ending posiiton of the current block + Index block_size = end - start; // available room in the current block + Index tail_size = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end; + + Index free_size = m_matrix.isCompressed() + ? Index(matrix.data().allocatedSize()) + block_size + : block_size; + + if(nnz>free_size) + { + // realloc manually to reduce copies + typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz); + + std::memcpy(newdata.valuePtr(), m_matrix.data().valuePtr(), start*sizeof(Scalar)); + std::memcpy(newdata.indexPtr(), m_matrix.data().indexPtr(), start*sizeof(Index)); + + std::memcpy(newdata.valuePtr() + start, tmp.data().valuePtr(), nnz*sizeof(Scalar)); + std::memcpy(newdata.indexPtr() + start, tmp.data().indexPtr(), nnz*sizeof(Index)); + + std::memcpy(newdata.valuePtr()+start+nnz, matrix.data().valuePtr()+end, tail_size*sizeof(Scalar)); + std::memcpy(newdata.indexPtr()+start+nnz, matrix.data().indexPtr()+end, tail_size*sizeof(Index)); + + newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz); + + matrix.data().swap(newdata); + } + else + { + // no need to realloc, simply copy the tail at its respective position and insert tmp + matrix.data().resize(start + nnz + tail_size); + + std::memmove(matrix.data().valuePtr()+start+nnz, matrix.data().valuePtr()+end, tail_size*sizeof(Scalar)); + std::memmove(matrix.data().indexPtr()+start+nnz, matrix.data().indexPtr()+end, tail_size*sizeof(Index)); + + std::memcpy(matrix.data().valuePtr()+start, tmp.data().valuePtr(), nnz*sizeof(Scalar)); + std::memcpy(matrix.data().indexPtr()+start, tmp.data().indexPtr(), nnz*sizeof(Index)); + } + + // update innerNonZeros + if(!m_matrix.isCompressed()) + for(Index j=0; j(other); + } + + inline const Scalar* valuePtr() const + { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + inline Scalar* valuePtr() + { return m_matrix.const_cast_derived().valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* innerIndexPtr() const + { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + inline Index* innerIndexPtr() + { return m_matrix.const_cast_derived().innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* outerIndexPtr() const + { return m_matrix.outerIndexPtr() + m_outerStart; } + inline Index* outerIndexPtr() + { return m_matrix.const_cast_derived().outerIndexPtr() + m_outerStart; } + + Index nonZeros() const + { + if(m_matrix.isCompressed()) + return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) + - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); + else if(m_outerSize.value()==0) + return 0; + else + return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); + } + + inline Scalar& coeffRef(int row, int col) + { + return m_matrix.const_cast_derived().coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); + eigen_assert(nonZeros()>0); + if(m_matrix.isCompressed()) + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; + else + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + typename SparseMatrixType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + +}; + + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public SparseMatrixBase,BlockRows,BlockCols,true> > +{ + typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; + typedef typename internal::remove_all::type _MatrixTypeNested; +public: + typedef Block BlockType; + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) +protected: + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + + class InnerIterator: public SparseMatrixType::InnerIterator + { + public: + inline InnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator + { + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const SparseMatrixType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + inline const Scalar* valuePtr() const + { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* innerIndexPtr() const + { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* outerIndexPtr() const + { return m_matrix.outerIndexPtr() + m_outerStart; } + + Index nonZeros() const + { + if(m_matrix.isCompressed()) + return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) + - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); + else if(m_outerSize.value()==0) + return 0; + else + return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); + eigen_assert(nonZeros()>0); + if(m_matrix.isCompressed()) + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; + else + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + + typename SparseMatrixType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; +}; + +//---------- + +/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this + * is col-major (resp. row-major). + */ +template +typename SparseMatrixBase::InnerVectorReturnType SparseMatrixBase::innerVector(Index outer) +{ return InnerVectorReturnType(derived(), outer); } + +/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this + * is col-major (resp. row-major). Read-only. + */ +template +const typename SparseMatrixBase::ConstInnerVectorReturnType SparseMatrixBase::innerVector(Index outer) const +{ return ConstInnerVectorReturnType(derived(), outer); } + +/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this + * is col-major (resp. row-major). + */ +template +typename SparseMatrixBase::InnerVectorsReturnType +SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) +{ + return Block(derived(), + IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, + IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize); + +} + +/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this + * is col-major (resp. row-major). Read-only. + */ +template +const typename SparseMatrixBase::ConstInnerVectorsReturnType +SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) const +{ + return Block(derived(), + IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, + IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize); + +} + +namespace internal { + +template< typename XprType, int BlockRows, int BlockCols, bool InnerPanel, + bool OuterVector = (BlockCols==1 && XprType::IsRowMajor) || (BlockRows==1 && !XprType::IsRowMajor)> +class GenericSparseBlockInnerIteratorImpl; + +} + +/** Generic implementation of sparse Block expression. + * Real-only. + */ +template +class BlockImpl + : public SparseMatrixBase >, internal::no_assignment_operator +{ + typedef typename internal::remove_all::type _MatrixTypeNested; + public: + typedef Block BlockType; + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) + + /** Column or Row constructor + */ + inline BlockImpl(const XprType& xpr, int i) + : m_matrix(xpr), + m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0), + m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0), + m_blockRows(BlockRows==1 ? 1 : xpr.rows()), + m_blockCols(BlockCols==1 ? 1 : xpr.cols()) + {} + + /** Dynamic-size constructor + */ + inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_startRow(startRow), m_startCol(startCol), m_blockRows(blockRows), m_blockCols(blockCols) + {} + + inline int rows() const { return m_blockRows.value(); } + inline int cols() const { return m_blockCols.value(); } + + inline Scalar& coeffRef(int row, int col) + { + return m_matrix.const_cast_derived() + .coeffRef(row + m_startRow.value(), col + m_startCol.value()); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value()); + } + + inline Scalar& coeffRef(int index) + { + return m_matrix.const_cast_derived() + .coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix + .coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + inline const _MatrixTypeNested& nestedExpression() const { return m_matrix; } + + typedef internal::GenericSparseBlockInnerIteratorImpl InnerIterator; + + class ReverseInnerIterator : public _MatrixTypeNested::ReverseInnerIterator + { + typedef typename _MatrixTypeNested::ReverseInnerIterator Base; + const BlockType& m_block; + Index m_begin; + public: + + EIGEN_STRONG_INLINE ReverseInnerIterator(const BlockType& block, Index outer) + : Base(block.derived().nestedExpression(), outer + (IsRowMajor ? block.m_startRow.value() : block.m_startCol.value())), + m_block(block), + m_begin(IsRowMajor ? block.m_startCol.value() : block.m_startRow.value()) + { + while( (Base::operator bool()) && (Base::index() >= (IsRowMajor ? m_block.m_startCol.value()+block.m_blockCols.value() : m_block.m_startRow.value()+block.m_blockRows.value())) ) + Base::operator--(); + } + + inline Index index() const { return Base::index() - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } + inline Index outer() const { return Base::outer() - (IsRowMajor ? m_block.m_startRow.value() : m_block.m_startCol.value()); } + inline Index row() const { return Base::row() - m_block.m_startRow.value(); } + inline Index col() const { return Base::col() - m_block.m_startCol.value(); } + + inline operator bool() const { return Base::operator bool() && Base::index() >= m_begin; } + }; + protected: + friend class internal::GenericSparseBlockInnerIteratorImpl; + friend class ReverseInnerIterator; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + + typename XprType::Nested m_matrix; + const internal::variable_if_dynamic m_startRow; + const internal::variable_if_dynamic m_startCol; + const internal::variable_if_dynamic m_blockRows; + const internal::variable_if_dynamic m_blockCols; + private: + Index nonZeros() const; +}; + +namespace internal { + template + class GenericSparseBlockInnerIteratorImpl : public internal::remove_all::type::InnerIterator + { + public: + typedef Block BlockType; + enum { + IsRowMajor = BlockType::IsRowMajor + }; + typedef typename BlockType::Index Index; + protected: + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef typename _MatrixTypeNested::InnerIterator Base; + const BlockType& m_block; + Index m_end; + public: + + EIGEN_STRONG_INLINE GenericSparseBlockInnerIteratorImpl(const BlockType& block, Index outer) + : Base(block.derived().nestedExpression(), outer + (IsRowMajor ? block.m_startRow.value() : block.m_startCol.value())), + m_block(block), + m_end(IsRowMajor ? block.m_startCol.value()+block.m_blockCols.value() : block.m_startRow.value()+block.m_blockRows.value()) + { + while( (Base::operator bool()) && (Base::index() < (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value())) ) + Base::operator++(); + } + + inline Index index() const { return Base::index() - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } + inline Index outer() const { return Base::outer() - (IsRowMajor ? m_block.m_startRow.value() : m_block.m_startCol.value()); } + inline Index row() const { return Base::row() - m_block.m_startRow.value(); } + inline Index col() const { return Base::col() - m_block.m_startCol.value(); } + + inline operator bool() const { return Base::operator bool() && Base::index() < m_end; } + }; + + // Row vector of a column-major sparse matrix or column of a row-major one. + template + class GenericSparseBlockInnerIteratorImpl + { + public: + typedef Block BlockType; + enum { + IsRowMajor = BlockType::IsRowMajor + }; + typedef typename BlockType::Index Index; + typedef typename BlockType::Scalar Scalar; + protected: + typedef typename internal::remove_all::type _MatrixTypeNested; + const BlockType& m_block; + Index m_outerPos; + Index m_innerIndex; + Scalar m_value; + Index m_end; + public: + + EIGEN_STRONG_INLINE GenericSparseBlockInnerIteratorImpl(const BlockType& block, Index outer = 0) + : + m_block(block), + m_outerPos( (IsRowMajor ? block.m_startCol.value() : block.m_startRow.value()) - 1), // -1 so that operator++ finds the first non-zero entry + m_innerIndex(IsRowMajor ? block.m_startRow.value() : block.m_startCol.value()), + m_end(IsRowMajor ? block.m_startCol.value()+block.m_blockCols.value() : block.m_startRow.value()+block.m_blockRows.value()) + { + EIGEN_UNUSED_VARIABLE(outer); + eigen_assert(outer==0); + + ++(*this); + } + + inline Index index() const { return m_outerPos - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } + inline Index outer() const { return 0; } + inline Index row() const { return IsRowMajor ? 0 : index(); } + inline Index col() const { return IsRowMajor ? index() : 0; } + + inline Scalar value() const { return m_value; } + + inline GenericSparseBlockInnerIteratorImpl& operator++() + { + // search next non-zero entry + while(m_outerPosm_data.resize(rows()); - Eigen::Map >(&this->m_data.index(0), rows()).setLinSpaced(0, rows()-1); - Eigen::Map >(&this->m_data.value(0), rows()).setOnes(); + Eigen::Map >(this->m_data.indexPtr(), rows()).setLinSpaced(0, rows()-1); + Eigen::Map >(this->m_data.valuePtr(), rows()).setOnes(); Eigen::Map >(this->m_outerIndex, rows()+1).setLinSpaced(0, rows()); std::free(m_innerNonZeros); m_innerNonZeros = 0; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h index 9341d9ad2..6f4a47cf5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h @@ -38,6 +38,7 @@ template class SparseMatrixBase typedef typename internal::packet_traits::type PacketScalar; typedef typename internal::traits::StorageKind StorageKind; typedef typename internal::traits::Index Index; + typedef typename internal::traits::Index StorageIndex; typedef typename internal::add_const_on_value_type_if_arithmetic< typename internal::packet_traits::type >::type PacketReturnType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h index f3da93a71..51ed9aeb1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h @@ -29,7 +29,10 @@ typename internal::traits >::Scalar SparseMatrix<_Scalar,_Options,_Index>::sum() const { eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); - return Matrix::Map(&m_data.value(0), m_data.size()).sum(); + if(this->isCompressed()) + return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); + else + return Base::sum(); } template @@ -37,7 +40,7 @@ typename internal::traits >::Scalar SparseVector<_Scalar,_Options,_Index>::sum() const { eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); - return Matrix::Map(&m_data.value(0), m_data.size()).sum(); + return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); } } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h index fcc18f5c9..55b84a4eb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h @@ -48,7 +48,7 @@ static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& r res.resize(rows, cols); res.reserve(estimated_nnz_prod); - double ratioColRes = double(estimated_nnz_prod)/double(lhs.rows()*rhs.cols()); + double ratioColRes = double(estimated_nnz_prod)/(double(lhs.rows())*double(rhs.cols())); for (Index j=0; j + UmfPackLU(const InputMatrixType& matrix) { init(); compute(matrix); diff --git a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh index f1a88ff74..86a8438cd 100644 --- a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh +++ b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh @@ -44,15 +44,10 @@ #define BTL_ASM_COMMENT(X) #endif -#if (defined __GNUC__) && (!defined __INTEL_COMPILER) && !defined(__arm__) && !defined(__powerpc__) -#define BTL_DISABLE_SSE_EXCEPTIONS() { \ - int aux; \ - asm( \ - "stmxcsr %[aux] \n\t" \ - "orl $32832, %[aux] \n\t" \ - "ldmxcsr %[aux] \n\t" \ - : : [aux] "m" (aux)); \ -} +#ifdef __SSE__ +#include "xmmintrin.h" +// This enables flush to zero (FTZ) and denormals are zero (DAZ) modes: +#define BTL_DISABLE_SSE_EXCEPTIONS() { _mm_setcsr(_mm_getcsr() | 0x8040); } #else #define BTL_DISABLE_SSE_EXCEPTIONS() #endif diff --git a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake index 2b11d8360..b5b62aee6 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake @@ -1,7 +1,7 @@ include(EigenTesting) include(CheckCXXSourceCompiles) -# configure the "site" and "buildname" +# configure the "site" and "buildname" ei_set_sitename() # retrieve and store the build string @@ -14,17 +14,10 @@ add_dependencies(check buildtests) # check whether /bin/bash exists find_file(EIGEN_BIN_BASH_EXISTS "/bin/bash" PATHS "/" NO_DEFAULT_PATH) -# CMake/Ctest does not allow us to change the build command, -# so we have to workaround by directly editing the generated DartConfiguration.tcl file -# save CMAKE_MAKE_PROGRAM -set(CMAKE_MAKE_PROGRAM_SAVE ${CMAKE_MAKE_PROGRAM}) -# and set a fake one -set(CMAKE_MAKE_PROGRAM "@EIGEN_MAKECOMMAND_PLACEHOLDER@") - # This call activates testing and generates the DartConfiguration.tcl include(CTest) -set(EIGEN_TEST_BUILD_FLAGS " " CACHE STRING "Options passed to the build command of unit tests") +set(EIGEN_TEST_BUILD_FLAGS "" CACHE STRING "Options passed to the build command of unit tests") # Overwrite default DartConfiguration.tcl such that ctest can build our unit tests. # Recall that our unit tests are not in the "all" target, so we have to explicitely ask ctest to build our custom 'buildtests' target. diff --git a/gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake b/gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake index 3c48d4c37..9246fa67c 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake @@ -26,7 +26,7 @@ function(DetermineShortWindowsName WIN_VERSION win_num_version) endfunction() function(DetermineOSVersion OS_VERSION) - if (WIN32) + if (WIN32 AND CMAKE_HOST_SYSTEM_NAME MATCHES Windows) file (TO_NATIVE_PATH "$ENV{COMSPEC}" SHELL) exec_program( ${SHELL} ARGS "/c" "ver" OUTPUT_VARIABLE ver_output) diff --git a/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox b/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox index 051a02ed9..b66e2ba2b 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox @@ -46,6 +46,9 @@ They are summarized in the following table: SPQR\link SPQRSupport_Module SPQRSupport \endlink QR factorization Any, rectangularfill-in reducing, multithreaded, fast dense algebra requires the SuiteSparse package, \b GPL recommended for linear least-squares problems, has a rank-revealing feature +PardisoLLT \n PardisoLDLT \n PardisoLU\link PardisoSupport_Module PardisoSupport \endlinkDirect LLt, LDLt, LU factorizationsSPD \n SPD \n SquareFill-in reducing, Leverage fast dense algebra, Multithreading + Requires the Intel MKL package, \b Proprietary + optimized for tough problems patterns, see also \link TopicUsingIntelMKL using MKL with Eigen \endlink Here \c SPD means symmetric positive definite. diff --git a/gtsam/3rdparty/Eigen/doc/TopicAssertions.dox b/gtsam/3rdparty/Eigen/doc/TopicAssertions.dox index 4ead40174..c8b4d84f2 100644 --- a/gtsam/3rdparty/Eigen/doc/TopicAssertions.dox +++ b/gtsam/3rdparty/Eigen/doc/TopicAssertions.dox @@ -16,7 +16,7 @@ Both eigen_assert and eigen_plain_assert are defined in Macros.h. Defining eigen #include #undef eigen_assert #define eigen_assert(x) \ - if (!x) { throw (std::runtime_error("Put your message here")); } + if (!(x)) { throw (std::runtime_error("Put your message here")); } \endcode \subsection DisableAssert Disabling assertions diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index c0d8a4e28..40c8f669d 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -125,6 +125,7 @@ endif(TEST_LIB) set_property(GLOBAL PROPERTY EIGEN_CURRENT_SUBPROJECT "Official") add_custom_target(BuildOfficial) +ei_add_test(rand) ei_add_test(meta) ei_add_test(sizeof) ei_add_test(dynalloc) diff --git a/gtsam/3rdparty/Eigen/test/commainitializer.cpp b/gtsam/3rdparty/Eigen/test/commainitializer.cpp index 99102b966..296592340 100644 --- a/gtsam/3rdparty/Eigen/test/commainitializer.cpp +++ b/gtsam/3rdparty/Eigen/test/commainitializer.cpp @@ -9,14 +9,69 @@ #include "main.h" + +template +void test_blocks() +{ + Matrix m_fixed; + MatrixXi m_dynamic(M1+M2, N1+N2); + + Matrix mat11; mat11.setRandom(); + Matrix mat12; mat12.setRandom(); + Matrix mat21; mat21.setRandom(); + Matrix mat22; mat22.setRandom(); + + MatrixXi matx11 = mat11, matx12 = mat12, matx21 = mat21, matx22 = mat22; + + { + VERIFY_IS_EQUAL((m_fixed << mat11, mat12, mat21, matx22).finished(), (m_dynamic << mat11, matx12, mat21, matx22).finished()); + VERIFY_IS_EQUAL((m_fixed.template topLeftCorner()), mat11); + VERIFY_IS_EQUAL((m_fixed.template topRightCorner()), mat12); + VERIFY_IS_EQUAL((m_fixed.template bottomLeftCorner()), mat21); + VERIFY_IS_EQUAL((m_fixed.template bottomRightCorner()), mat22); + VERIFY_IS_EQUAL((m_fixed << mat12, mat11, matx21, mat22).finished(), (m_dynamic << mat12, matx11, matx21, mat22).finished()); + } + + if(N1 > 0) + { + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat12, mat11, mat21, mat22)); + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat12, mat21, mat21, mat22)); + } + else + { + // allow insertion of zero-column blocks: + VERIFY_IS_EQUAL((m_fixed << mat11, mat12, mat11, mat11, mat21, mat21, mat22).finished(), (m_dynamic << mat12, mat22).finished()); + } + if(M1 != M2) + { + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat21, mat12, mat22)); + } +} + + +template +struct test_block_recursion +{ + static void run() + { + test_blocks<(N>>6)&3, (N>>4)&3, (N>>2)&3, N & 3>(); + test_block_recursion::run(); + } +}; + +template<> +struct test_block_recursion<-1> +{ + static void run() { } +}; + void test_commainitializer() { Matrix3d m3; Matrix4d m4; - VERIFY_RAISES_ASSERT( (m3 << 1, 2, 3, 4, 5, 6, 7, 8) ); - #ifndef _MSC_VER + VERIFY_RAISES_ASSERT( (m3 << 1, 2, 3, 4, 5, 6, 7, 8) ); VERIFY_RAISES_ASSERT( (m3 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10) ); #endif @@ -43,4 +98,7 @@ void test_commainitializer() 4, 5, 6, vec[2].transpose(); VERIFY_IS_APPROX(m3, ref); + + // recursively test all block-sizes from 0 to 3: + test_block_recursion<(1<<8) - 1>(); } diff --git a/gtsam/3rdparty/Eigen/test/cwiseop.cpp b/gtsam/3rdparty/Eigen/test/cwiseop.cpp index e3361da17..d13002cae 100644 --- a/gtsam/3rdparty/Eigen/test/cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/cwiseop.cpp @@ -164,9 +164,12 @@ template void cwiseops(const MatrixType& m) VERIFY( (m1.cwise().min(m2).cwise() < (m1+mones)).all() ); VERIFY( (m1.cwise().max(m2).cwise() > (m1-mones)).all() ); +#if(__cplusplus < 201103L) +// std::binder* are deprecated since c++11 and will be removed in c++17 VERIFY( (m1.cwise()(), Scalar(1)))).all() ); VERIFY( !(m1.cwise()(), Scalar(1)))).all() ); VERIFY( !(m1.cwise()>m1bis.unaryExpr(bind2nd(plus(), Scalar(1)))).any() ); +#endif cwiseops_real_only(m1, m2, m3, mones); } diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp index 22e1cc342..a36edd473 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp @@ -137,9 +137,12 @@ template void cwiseops(const MatrixType& m) VERIFY( (m1.cwise().min(m2).cwise() < (m1+mones)).all() ); VERIFY( (m1.cwise().max(m2).cwise() > (m1-mones)).all() ); +#if(__cplusplus < 201103L) +// std::binder* are deprecated since c++11 and will be removed in c++17 VERIFY( (m1.cwise()(), Scalar(1)))).all() ); VERIFY( !(m1.cwise()(), Scalar(1)))).all() ); VERIFY( !(m1.cwise()>m1.unaryExpr(bind2nd(plus(), Scalar(1)))).any() ); +#endif } void test_eigen2_cwiseop() diff --git a/gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp b/gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp index c91bde819..01330308a 100644 --- a/gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp @@ -54,6 +54,8 @@ template void homogeneous(void) T2MatrixType t2 = T2MatrixType::Random(); VERIFY_IS_APPROX(t2 * (v0.homogeneous().eval()), t2 * v0.homogeneous()); VERIFY_IS_APPROX(t2 * (m0.colwise().homogeneous().eval()), t2 * m0.colwise().homogeneous()); + VERIFY_IS_APPROX(t2 * (v0.homogeneous().asDiagonal()), t2 * hv0.asDiagonal()); + VERIFY_IS_APPROX((v0.homogeneous().asDiagonal()) * t2, hv0.asDiagonal() * t2); VERIFY_IS_APPROX((v0.transpose().rowwise().homogeneous().eval()) * t2, v0.transpose().rowwise().homogeneous() * t2); diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index 547765714..383c42bad 100644 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -320,6 +320,9 @@ template void transformations() t0.scale(v0); t1 *= AlignedScaling3(v0); VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1)); + t1 = t1 * v0.asDiagonal(); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); // transformation * translation t0.translate(v0); t1 = t1 * Translation3(v0); @@ -437,6 +440,79 @@ template void transformations() Rotation2D r2(r1); // copy ctor VERIFY_IS_APPROX(r2.angle(),s0); } + + { + Transform3 t32(Matrix4::Random()), t33, t34; + t34 = t33 = t32; + t32.scale(v0); + t33*=AlignedScaling3(v0); + VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); + t33 = t34 * AlignedScaling3(v0); + VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); + } + +} + +template +void transform_associativity_left(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) +{ + VERIFY_IS_APPROX( q*(a1*v), (q*a1)*v ); + VERIFY_IS_APPROX( q*(a2*v), (q*a2)*v ); + VERIFY_IS_APPROX( q*(p*h).hnormalized(), ((q*p)*h).hnormalized() ); +} + +template +void transform_associativity2(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) +{ + VERIFY_IS_APPROX( a1*(q*v), (a1*q)*v ); + VERIFY_IS_APPROX( a2*(q*v), (a2*q)*v ); + VERIFY_IS_APPROX( p *(q*v).homogeneous(), (p *q)*v.homogeneous() ); + + transform_associativity_left(a1, a2,p, q, v, h); +} + +template +void transform_associativity(const RotationType& R) +{ + typedef Matrix VectorType; + typedef Matrix HVectorType; + typedef Matrix LinearType; + typedef Matrix MatrixType; + typedef Transform AffineCompactType; + typedef Transform AffineType; + typedef Transform ProjectiveType; + typedef DiagonalMatrix ScalingType; + typedef Translation TranslationType; + + AffineCompactType A1c; A1c.matrix().setRandom(); + AffineCompactType A2c; A2c.matrix().setRandom(); + AffineType A1(A1c); + AffineType A2(A2c); + ProjectiveType P1; P1.matrix().setRandom(); + VectorType v1 = VectorType::Random(); + VectorType v2 = VectorType::Random(); + HVectorType h1 = HVectorType::Random(); + Scalar s1 = internal::random(); + LinearType L = LinearType::Random(); + MatrixType M = MatrixType::Random(); + + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2c, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, v1.asDiagonal(), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, ScalingType(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(s1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, TranslationType(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity_left(A1c, A1, P1, L, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, R, v2, h1) ); + + VERIFY_IS_APPROX( A1*(M*h1), (A1*M)*h1 ); + VERIFY_IS_APPROX( A1c*(M*h1), (A1c*M)*h1 ); + VERIFY_IS_APPROX( P1*(M*h1), (P1*M)*h1 ); + + VERIFY_IS_APPROX( M*(A1*h1), (M*A1)*h1 ); + VERIFY_IS_APPROX( M*(A1c*h1), (M*A1c)*h1 ); + VERIFY_IS_APPROX( M*(P1*h1), ((M*P1)*h1) ); } template void transform_alignment() @@ -517,5 +593,8 @@ void test_geo_transformations() CALL_SUBTEST_7(( transform_products() )); CALL_SUBTEST_7(( transform_products() )); + + CALL_SUBTEST_8(( transform_associativity(Rotation2D(internal::random()*double(3.14))) )); + CALL_SUBTEST_8(( transform_associativity(Quaterniond(Vector4d::Random().normalized())) )); } } diff --git a/gtsam/3rdparty/Eigen/test/packetmath.cpp b/gtsam/3rdparty/Eigen/test/packetmath.cpp index 38aa256ce..bac7b02d1 100644 --- a/gtsam/3rdparty/Eigen/test/packetmath.cpp +++ b/gtsam/3rdparty/Eigen/test/packetmath.cpp @@ -327,6 +327,7 @@ template void test_conj_helper(Scalar ref[i] += cj0(data1[i]) * cj1(data2[i]); VERIFY(internal::isApprox(ref[i], cj.pmadd(data1[i],data2[i],tmp)) && "conj_helper pmadd"); } + *pval += 0; // Workaround msvc 2013 issue (bad code generation) internal::pstore(pval,pcj.pmadd(internal::pload(data1),internal::pload(data2),internal::pload(pval))); VERIFY(areApprox(ref, pval, PacketSize) && "conj_helper pmadd"); } diff --git a/gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp b/gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp index c4ef2d4bd..eb6ad18c9 100644 --- a/gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp +++ b/gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp @@ -53,14 +53,29 @@ template void inverse_general_4x4(int repeat) // FIXME that 1.25 used to be 1.2 until we tested gcc 4.1 on 30 June 2010 and got 1.21. VERIFY(error_avg < (NumTraits::IsComplex ? 8.0 : 1.25)); VERIFY(error_max < (NumTraits::IsComplex ? 64.0 : 20.0)); + + { + int s = 5;//internal::random(4,10); + int i = 0;//internal::random(0,s-4); + int j = 0;//internal::random(0,s-4); + Matrix mat(s,s); + mat.setRandom(); + MatrixType submat = mat.template block<4,4>(i,j); + MatrixType mat_inv = mat.template block<4,4>(i,j).inverse(); + VERIFY_IS_APPROX(mat_inv, submat.inverse()); + mat.template block<4,4>(i,j) = submat.inverse(); + VERIFY_IS_APPROX(mat_inv, (mat.template block<4,4>(i,j))); + } } void test_prec_inverse_4x4() { CALL_SUBTEST_1((inverse_permutation_4x4())); CALL_SUBTEST_1(( inverse_general_4x4(200000 * g_repeat) )); + CALL_SUBTEST_1(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_2((inverse_permutation_4x4 >())); + CALL_SUBTEST_2(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_2(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_3((inverse_permutation_4x4())); diff --git a/gtsam/3rdparty/Eigen/test/product.h b/gtsam/3rdparty/Eigen/test/product.h index 0d054ff46..397af24ae 100644 --- a/gtsam/3rdparty/Eigen/test/product.h +++ b/gtsam/3rdparty/Eigen/test/product.h @@ -178,4 +178,12 @@ template void product(const MatrixType& m) // CwiseUnaryOp VERIFY_IS_APPROX(x = Scalar(1.)*(A*x), A*z); } + + // regression for blas_trais + { + VERIFY_IS_APPROX(square * (square*square).transpose(), square * square.transpose() * square.transpose()); + VERIFY_IS_APPROX(square * (-(square*square)), -square * square * square); + VERIFY_IS_APPROX(square * (s1*(square*square)), s1 * square * square * square); + VERIFY_IS_APPROX(square * (square*square).conjugate(), square * square.conjugate() * square.conjugate()); + } } diff --git a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp index abe6a9d14..2df7b63a7 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp @@ -299,6 +299,14 @@ template void sparse_basic(const SparseMatrixType& re VERIFY_IS_APPROX(m1.innerVector(0).dot(refM2.row(0)), refM1.row(0).dot(refM2.row(0))); else VERIFY_IS_APPROX(m1.innerVector(0).dot(refM2.row(0)), refM1.col(0).dot(refM2.row(0))); + + DenseVector rv = DenseVector::Random(m1.cols()); + DenseVector cv = DenseVector::Random(m1.rows()); + Index r = internal::random(0,m1.rows()-2); + Index c = internal::random(0,m1.cols()-1); + VERIFY_IS_APPROX(( m1.template block<1,Dynamic>(r,0,1,m1.cols()).dot(rv)) , refM1.row(r).dot(rv)); + VERIFY_IS_APPROX(m1.row(r).dot(rv), refM1.row(r).dot(rv)); + VERIFY_IS_APPROX(m1.col(c).dot(cv), refM1.col(c).dot(cv)); VERIFY_IS_APPROX(m1.conjugate(), refM1.conjugate()); VERIFY_IS_APPROX(m1.real(), refM1.real()); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h index 6825a7882..88dba54f5 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h @@ -293,7 +293,7 @@ void MatrixExponential::computeUV(float) const float maxnorm = 3.925724783138660f; frexp(m_l1norm / maxnorm, &m_squarings); if (m_squarings < 0) m_squarings = 0; - MatrixType A = m_M / pow(Scalar(2), m_squarings); + MatrixType A = m_M / Scalar(pow(2, m_squarings)); pade7(A); } } @@ -315,7 +315,7 @@ void MatrixExponential::computeUV(double) const double maxnorm = 5.371920351148152; frexp(m_l1norm / maxnorm, &m_squarings); if (m_squarings < 0) m_squarings = 0; - MatrixType A = m_M / pow(Scalar(2), m_squarings); + MatrixType A = m_M / Scalar(pow(2, m_squarings)); pade13(A); } } @@ -340,7 +340,7 @@ void MatrixExponential::computeUV(long double) const long double maxnorm = 4.0246098906697353063L; frexp(m_l1norm / maxnorm, &m_squarings); if (m_squarings < 0) m_squarings = 0; - MatrixType A = m_M / pow(Scalar(2), m_squarings); + MatrixType A = m_M / Scalar(pow(2, m_squarings)); pade13(A); } #elif LDBL_MANT_DIG <= 106 // double-double @@ -376,7 +376,7 @@ void MatrixExponential::computeUV(long double) const long double maxnorm = 2.884233277829519311757165057717815L; frexp(m_l1norm / maxnorm, &m_squarings); if (m_squarings < 0) m_squarings = 0; - MatrixType A = m_M / pow(Scalar(2), m_squarings); + MatrixType A = m_M / Scalar(pow(2, m_squarings)); pade17(A); } #else diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h index 49db8d35d..9ea23a9a1 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h @@ -31,6 +31,8 @@ namespace Eigen enum { OrderAtCompileTime = _Degree==Dynamic ? Dynamic : _Degree+1 /*!< The spline curve's order at compile-time. */ }; enum { NumOfDerivativesAtCompileTime = OrderAtCompileTime /*!< The number of derivatives defined for the current spline. */ }; + + enum { DerivativeMemoryLayout = Dimension==1 ? RowMajor : ColMajor /*!< The derivative type's memory layout. */ }; /** \brief The data type used to store non-zero basis functions. */ typedef Array BasisVectorType; @@ -39,7 +41,7 @@ namespace Eigen typedef Array BasisDerivativeType; /** \brief The data type used to store the spline's derivative values. */ - typedef Array DerivativeType; + typedef Array DerivativeType; /** \brief The point type the spline is representing. */ typedef Array PointType; @@ -62,12 +64,14 @@ namespace Eigen { enum { OrderAtCompileTime = _Degree==Dynamic ? Dynamic : _Degree+1 /*!< The spline curve's order at compile-time. */ }; enum { NumOfDerivativesAtCompileTime = _DerivativeOrder==Dynamic ? Dynamic : _DerivativeOrder+1 /*!< The number of derivatives defined for the current spline. */ }; + + enum { DerivativeMemoryLayout = _Dim==1 ? RowMajor : ColMajor /*!< The derivative type's memory layout. */ }; /** \brief The data type used to store the values of the basis function derivatives. */ typedef Array<_Scalar,Dynamic,Dynamic,RowMajor,NumOfDerivativesAtCompileTime,OrderAtCompileTime> BasisDerivativeType; /** \brief The data type used to store the spline's derivative values. */ - typedef Array<_Scalar,_Dim,Dynamic,ColMajor,_Dim,NumOfDerivativesAtCompileTime> DerivativeType; + typedef Array<_Scalar,_Dim,Dynamic,DerivativeMemoryLayout,_Dim,NumOfDerivativesAtCompileTime> DerivativeType; }; /** \brief 2D float B-spline with dynamic degree. */ diff --git a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp index 087e7c542..7c112a1f0 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp @@ -162,6 +162,15 @@ void test_autodiff_jacobian() CALL_SUBTEST(( forward_jacobian(TestFunc1(3,3)) )); } +double bug_1222() { + typedef Eigen::AutoDiffScalar AD; + const double _cv1_3 = 1.0; + const AD chi_3 = 1.0; + // this line did not work, because operator+ returns ADS, which then cannot be converted to ADS + const AD denom = chi_3 + _cv1_3; + return denom.value(); +} + void test_autodiff() { for(int i = 0; i < g_repeat; i++) { @@ -169,5 +178,7 @@ void test_autodiff() CALL_SUBTEST_2( test_autodiff_vector() ); CALL_SUBTEST_3( test_autodiff_jacobian() ); } + + bug_1222(); } diff --git a/gtsam/3rdparty/SuiteSparse_config/Makefile b/gtsam/3rdparty/SuiteSparse_config/Makefile new file mode 100644 index 000000000..aa858aeab --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/Makefile @@ -0,0 +1,72 @@ +#------------------------------------------------------------------------------- +# SuiteSparse_config Makefile +#------------------------------------------------------------------------------- + +SUITESPARSE ?= $(realpath $(CURDIR)/..) +export SUITESPARSE + +# version of SuiteSparse_config is also version of SuiteSparse meta-package +LIBRARY = libsuitesparseconfig +VERSION = 4.5.6 +SO_VERSION = 4 + +default: library + +include SuiteSparse_config.mk + +ccode: all + +all: library + +# compile and install in SuiteSparse/lib +library: $(AR_TARGET) + $(MAKE) install INSTALL=$(SUITESPARSE) + +OBJ = SuiteSparse_config.o + +SuiteSparse_config.o: SuiteSparse_config.c SuiteSparse_config.h + $(CC) $(CF) -c SuiteSparse_config.c + +static: $(AR_TARGET) + +$(AR_TARGET): $(OBJ) + $(ARCHIVE) $(AR_TARGET) SuiteSparse_config.o + $(RANLIB) $(AR_TARGET) + +distclean: purge + +purge: clean + ( cd xerbla ; $(MAKE) purge ) + - $(RM) -r $(PURGE) + +clean: + ( cd xerbla ; $(MAKE) clean ) + - $(RM) -r $(CLEAN) + +# install SuiteSparse_config +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(OBJ) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) SuiteSparse_config.h $(INSTALL_INCLUDE) + $(CP) README.txt $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 755 $(INSTALL_LIB)/$(SO_PLAIN) + chmod 644 $(INSTALL_INCLUDE)/SuiteSparse_config.h + chmod 644 $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + +# uninstall SuiteSparse_config +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_LIB)/$(SO_MAIN) + $(RM) $(INSTALL_INCLUDE)/SuiteSparse_config.h + $(RM) $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + ( cd xerbla ; $(MAKE) uninstall ) + + diff --git a/gtsam/3rdparty/SuiteSparse_config/README.txt b/gtsam/3rdparty/SuiteSparse_config/README.txt new file mode 100644 index 000000000..8129f5a04 --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/README.txt @@ -0,0 +1,51 @@ +SuiteSparse_config, 2017, Timothy A. Davis, http://www.suitesparse.com +(formerly the UFconfig package) + +This directory contains a default SuiteSparse_config.mk file. It tries to +detect your system (Linux, SunOS, or Mac), which compiler to use (icc or cc), +which BLAS and LAPACK library to use (OpenBLAS or MKL), and whether or not to +compile with CUDA. + +For alternatives, see the comments in the SuiteSparse_config.mk file. + +License: No licensing restrictions apply to this file or to the +SuiteSparse_config directory. + +-------------------------------------------------------------------------------- + +SuiteSparse_config contains configuration settings for all many of the software +packages that I develop or co-author. Note that older versions of some of +these packages do not require SuiteSparse_config. + + Package Description + ------- ----------- + AMD approximate minimum degree ordering + CAMD constrained AMD + COLAMD column approximate minimum degree ordering + CCOLAMD constrained approximate minimum degree ordering + UMFPACK sparse LU factorization, with the BLAS + CXSparse int/long/real/complex version of CSparse + CHOLMOD sparse Cholesky factorization, update/downdate + KLU sparse LU factorization, BLAS-free + BTF permutation to block triangular form + LDL concise sparse LDL' + LPDASA LP Dual Active Set Algorithm + RBio read/write files in Rutherford/Boeing format + SPQR sparse QR factorization (full name: SuiteSparseQR) + +SuiteSparse_config is not required by these packages: + + CSparse a Concise Sparse matrix package + MATLAB_Tools toolboxes for use in MATLAB + +In addition, the xerbla/ directory contains Fortan and C versions of the +BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to +the BLAS or LAPACK. The xerbla provided here does not print any message, so +the entire Fortran I/O library does not need to be linked into a C application. +Most versions of the BLAS contain xerbla, but those from K. Goto do not. Use +this if you need too. + +If you edit this directory (SuiteSparse_config.mk in particular) then you +must do "make purge ; make" in the parent directory to recompile all of +SuiteSparse. Otherwise, the changes will not necessarily be applied. + diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c new file mode 100644 index 000000000..b491539fe --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c @@ -0,0 +1,531 @@ +/* ========================================================================== */ +/* === SuiteSparse_config =================================================== */ +/* ========================================================================== */ + +/* SuiteSparse configuration : memory manager and printf functions. */ + +/* Copyright (c) 2013, Timothy A. Davis. No licensing restrictions + * apply to this file or to the SuiteSparse_config directory. + * Author: Timothy A. Davis. + */ + +#include +#include + +#ifndef NPRINT +#include +#endif + +#ifdef MATLAB_MEX_FILE +#include "mex.h" +#include "matrix.h" +#endif + +#ifndef NULL +#define NULL ((void *) 0) +#endif + +#include "SuiteSparse_config.h" + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_config : a global extern struct */ +/* -------------------------------------------------------------------------- */ + +/* The SuiteSparse_config struct is available to all SuiteSparse functions and + to all applications that use those functions. It must be modified with + care, particularly in a multithreaded context. Normally, the application + will initialize this object once, via SuiteSparse_start, possibily followed + by application-specific modifications if the applications wants to use + alternative memory manager functions. + + The user can redefine these global pointers at run-time to change the + memory manager and printf function used by SuiteSparse. + + If -DNMALLOC is defined at compile-time, then no memory-manager is + specified. You must define them at run-time, after calling + SuiteSparse_start. + + If -DPRINT is defined a compile time, then printf is disabled, and + SuiteSparse will not use printf. + */ + +struct SuiteSparse_config_struct SuiteSparse_config = +{ + + /* memory management functions */ + #ifndef NMALLOC + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + mxMalloc, mxCalloc, mxRealloc, mxFree, + #else + /* standard ANSI C: */ + malloc, calloc, realloc, free, + #endif + #else + /* no memory manager defined; you must define one at run-time: */ + NULL, NULL, NULL, NULL, + #endif + + /* printf function */ + #ifndef NPRINT + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + mexPrintf, + #else + /* standard ANSI C: */ + printf, + #endif + #else + /* printf is disabled */ + NULL, + #endif + + SuiteSparse_hypot, + SuiteSparse_divcomplex + +} ; + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_start */ +/* -------------------------------------------------------------------------- */ + +/* All applications that use SuiteSparse should call SuiteSparse_start prior + to using any SuiteSparse function. Only a single thread should call this + function, in a multithreaded application. Currently, this function is + optional, since all this function currently does is to set the four memory + function pointers to NULL (which tells SuiteSparse to use the default + functions). In a multi- threaded application, only a single thread should + call this function. + + Future releases of SuiteSparse might enforce a requirement that + SuiteSparse_start be called prior to calling any SuiteSparse function. + */ + +void SuiteSparse_start ( void ) +{ + + /* memory management functions */ + #ifndef NMALLOC + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + SuiteSparse_config.malloc_func = mxMalloc ; + SuiteSparse_config.calloc_func = mxCalloc ; + SuiteSparse_config.realloc_func = mxRealloc ; + SuiteSparse_config.free_func = mxFree ; + #else + /* standard ANSI C: */ + SuiteSparse_config.malloc_func = malloc ; + SuiteSparse_config.calloc_func = calloc ; + SuiteSparse_config.realloc_func = realloc ; + SuiteSparse_config.free_func = free ; + #endif + #else + /* no memory manager defined; you must define one after calling + SuiteSparse_start */ + SuiteSparse_config.malloc_func = NULL ; + SuiteSparse_config.calloc_func = NULL ; + SuiteSparse_config.realloc_func = NULL ; + SuiteSparse_config.free_func = NULL ; + #endif + + /* printf function */ + #ifndef NPRINT + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + SuiteSparse_config.printf_func = mexPrintf ; + #else + /* standard ANSI C: */ + SuiteSparse_config.printf_func = printf ; + #endif + #else + /* printf is disabled */ + SuiteSparse_config.printf_func = NULL ; + #endif + + /* math functions */ + SuiteSparse_config.hypot_func = SuiteSparse_hypot ; + SuiteSparse_config.divcomplex_func = SuiteSparse_divcomplex ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_finish */ +/* -------------------------------------------------------------------------- */ + +/* This currently does nothing, but in the future, applications should call + SuiteSparse_start before calling any SuiteSparse function, and then + SuiteSparse_finish after calling the last SuiteSparse function, just before + exiting. In a multithreaded application, only a single thread should call + this function. + + Future releases of SuiteSparse might use this function for any + SuiteSparse-wide cleanup operations or finalization of statistics. + */ + +void SuiteSparse_finish ( void ) +{ + /* do nothing */ ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_malloc: malloc wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_malloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to malloc */ + size_t size_of_item /* sizeof each item */ +) +{ + void *p ; + size_t size ; + if (nitems < 1) nitems = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems * size_of_item ; + + if (size != ((double) nitems) * size_of_item) + { + /* size_t overflow */ + p = NULL ; + } + else + { + p = (void *) (SuiteSparse_config.malloc_func) (size) ; + } + return (p) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_calloc: calloc wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_calloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to calloc */ + size_t size_of_item /* sizeof each item */ +) +{ + void *p ; + size_t size ; + if (nitems < 1) nitems = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems * size_of_item ; + + if (size != ((double) nitems) * size_of_item) + { + /* size_t overflow */ + p = NULL ; + } + else + { + p = (void *) (SuiteSparse_config.calloc_func) (nitems, size_of_item) ; + } + return (p) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_realloc: realloc wrapper */ +/* -------------------------------------------------------------------------- */ + +/* If p is non-NULL on input, it points to a previously allocated object of + size nitems_old * size_of_item. The object is reallocated to be of size + nitems_new * size_of_item. If p is NULL on input, then a new object of that + size is allocated. On success, a pointer to the new object is returned, + and ok is returned as 1. If the allocation fails, ok is set to 0 and a + pointer to the old (unmodified) object is returned. + */ + +void *SuiteSparse_realloc /* pointer to reallocated block of memory, or + to original block if the realloc failed. */ +( + size_t nitems_new, /* new number of items in the object */ + size_t nitems_old, /* old number of items in the object */ + size_t size_of_item, /* sizeof each item */ + void *p, /* old object to reallocate */ + int *ok /* 1 if successful, 0 otherwise */ +) +{ + size_t size ; + if (nitems_old < 1) nitems_old = 1 ; + if (nitems_new < 1) nitems_new = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems_new * size_of_item ; + + if (size != ((double) nitems_new) * size_of_item) + { + /* size_t overflow */ + (*ok) = 0 ; + } + else if (p == NULL) + { + /* a fresh object is being allocated */ + p = SuiteSparse_malloc (nitems_new, size_of_item) ; + (*ok) = (p != NULL) ; + } + else if (nitems_old == nitems_new) + { + /* the object does not change; do nothing */ + (*ok) = 1 ; + } + else + { + /* change the size of the object from nitems_old to nitems_new */ + void *pnew ; + pnew = (void *) (SuiteSparse_config.realloc_func) (p, size) ; + if (pnew == NULL) + { + if (nitems_new < nitems_old) + { + /* the attempt to reduce the size of the block failed, but + the old block is unchanged. So pretend to succeed. */ + (*ok) = 1 ; + } + else + { + /* out of memory */ + (*ok) = 0 ; + } + } + else + { + /* success */ + p = pnew ; + (*ok) = 1 ; + } + } + return (p) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_free: free wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_free /* always returns NULL */ +( + void *p /* block to free */ +) +{ + if (p) + { + (SuiteSparse_config.free_func) (p) ; + } + return (NULL) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_tic: return current wall clock time */ +/* -------------------------------------------------------------------------- */ + +/* Returns the number of seconds (tic [0]) and nanoseconds (tic [1]) since some + * unspecified but fixed time in the past. If no timer is installed, zero is + * returned. A scalar double precision value for 'tic' could be used, but this + * might cause loss of precision because clock_getttime returns the time from + * some distant time in the past. Thus, an array of size 2 is used. + * + * The timer is enabled by default. To disable the timer, compile with + * -DNTIMER. If enabled on a POSIX C 1993 system, the timer requires linking + * with the -lrt library. + * + * example: + * + * double tic [2], r, s, t ; + * SuiteSparse_tic (tic) ; // start the timer + * // do some work A + * t = SuiteSparse_toc (tic) ; // t is time for work A, in seconds + * // do some work B + * s = SuiteSparse_toc (tic) ; // s is time for work A and B, in seconds + * SuiteSparse_tic (tic) ; // restart the timer + * // do some work C + * r = SuiteSparse_toc (tic) ; // s is time for work C, in seconds + * + * A double array of size 2 is used so that this routine can be more easily + * ported to non-POSIX systems. The caller does not rely on the POSIX + * include file. + */ + +#ifdef SUITESPARSE_TIMER_ENABLED + +#include + +void SuiteSparse_tic +( + double tic [2] /* output, contents undefined on input */ +) +{ + /* POSIX C 1993 timer, requires -librt */ + struct timespec t ; + clock_gettime (CLOCK_MONOTONIC, &t) ; + tic [0] = (double) (t.tv_sec) ; + tic [1] = (double) (t.tv_nsec) ; +} + +#else + +void SuiteSparse_tic +( + double tic [2] /* output, contents undefined on input */ +) +{ + /* no timer installed */ + tic [0] = 0 ; + tic [1] = 0 ; +} + +#endif + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_toc: return time since last tic */ +/* -------------------------------------------------------------------------- */ + +/* Assuming SuiteSparse_tic is accurate to the nanosecond, this function is + * accurate down to the nanosecond for 2^53 nanoseconds since the last call to + * SuiteSparse_tic, which is sufficient for SuiteSparse (about 104 days). If + * additional accuracy is required, the caller can use two calls to + * SuiteSparse_tic and do the calculations differently. + */ + +double SuiteSparse_toc /* returns time in seconds since last tic */ +( + double tic [2] /* input, not modified from last call to SuiteSparse_tic */ +) +{ + double toc [2] ; + SuiteSparse_tic (toc) ; + return ((toc [0] - tic [0]) + 1e-9 * (toc [1] - tic [1])) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_time: return current wallclock time in seconds */ +/* -------------------------------------------------------------------------- */ + +/* This function might not be accurate down to the nanosecond. */ + +double SuiteSparse_time /* returns current wall clock time in seconds */ +( + void +) +{ + double toc [2] ; + SuiteSparse_tic (toc) ; + return (toc [0] + 1e-9 * toc [1]) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_version: return the current version of SuiteSparse */ +/* -------------------------------------------------------------------------- */ + +int SuiteSparse_version +( + int version [3] +) +{ + if (version != NULL) + { + version [0] = SUITESPARSE_MAIN_VERSION ; + version [1] = SUITESPARSE_SUB_VERSION ; + version [2] = SUITESPARSE_SUBSUB_VERSION ; + } + return (SUITESPARSE_VERSION) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_hypot */ +/* -------------------------------------------------------------------------- */ + +/* There is an equivalent routine called hypot in , which conforms + * to ANSI C99. However, SuiteSparse does not assume that ANSI C99 is + * available. You can use the ANSI C99 hypot routine with: + * + * #include + *i SuiteSparse_config.hypot_func = hypot ; + * + * Default value of the SuiteSparse_config.hypot_func pointer is + * SuiteSparse_hypot, defined below. + * + * s = hypot (x,y) computes s = sqrt (x*x + y*y) but does so more accurately. + * The NaN cases for the double relops x >= y and x+y == x are safely ignored. + * + * Source: Algorithm 312, "Absolute value and square root of a complex number," + * P. Friedland, Comm. ACM, vol 10, no 10, October 1967, page 665. + */ + +double SuiteSparse_hypot (double x, double y) +{ + double s, r ; + x = fabs (x) ; + y = fabs (y) ; + if (x >= y) + { + if (x + y == x) + { + s = x ; + } + else + { + r = y / x ; + s = x * sqrt (1.0 + r*r) ; + } + } + else + { + if (y + x == y) + { + s = y ; + } + else + { + r = x / y ; + s = y * sqrt (1.0 + r*r) ; + } + } + return (s) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_divcomplex */ +/* -------------------------------------------------------------------------- */ + +/* c = a/b where c, a, and b are complex. The real and imaginary parts are + * passed as separate arguments to this routine. The NaN case is ignored + * for the double relop br >= bi. Returns 1 if the denominator is zero, + * 0 otherwise. + * + * This uses ACM Algo 116, by R. L. Smith, 1962, which tries to avoid + * underflow and overflow. + * + * c can be the same variable as a or b. + * + * Default value of the SuiteSparse_config.divcomplex_func pointer is + * SuiteSparse_divcomplex. + */ + +int SuiteSparse_divcomplex +( + double ar, double ai, /* real and imaginary parts of a */ + double br, double bi, /* real and imaginary parts of b */ + double *cr, double *ci /* real and imaginary parts of c */ +) +{ + double tr, ti, r, den ; + if (fabs (br) >= fabs (bi)) + { + r = bi / br ; + den = br + r * bi ; + tr = (ar + ai * r) / den ; + ti = (ai - ar * r) / den ; + } + else + { + r = br / bi ; + den = r * br + bi ; + tr = (ar * r + ai) / den ; + ti = (ai * r - ar) / den ; + } + *cr = tr ; + *ci = ti ; + return (den == 0.) ; +} diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h new file mode 100644 index 000000000..7d4de65d3 --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h @@ -0,0 +1,247 @@ +/* ========================================================================== */ +/* === SuiteSparse_config =================================================== */ +/* ========================================================================== */ + +/* Configuration file for SuiteSparse: a Suite of Sparse matrix packages + * (AMD, COLAMD, CCOLAMD, CAMD, CHOLMOD, UMFPACK, CXSparse, and others). + * + * SuiteSparse_config.h provides the definition of the long integer. On most + * systems, a C program can be compiled in LP64 mode, in which long's and + * pointers are both 64-bits, and int's are 32-bits. Windows 64, however, uses + * the LLP64 model, in which int's and long's are 32-bits, and long long's and + * pointers are 64-bits. + * + * SuiteSparse packages that include long integer versions are + * intended for the LP64 mode. However, as a workaround for Windows 64 + * (and perhaps other systems), the long integer can be redefined. + * + * If _WIN64 is defined, then the __int64 type is used instead of long. + * + * The long integer can also be defined at compile time. For example, this + * could be added to SuiteSparse_config.mk: + * + * CFLAGS = -O -D'SuiteSparse_long=long long' \ + * -D'SuiteSparse_long_max=9223372036854775801' -D'SuiteSparse_long_idd="lld"' + * + * This file defines SuiteSparse_long as either long (on all but _WIN64) or + * __int64 on Windows 64. The intent is that a SuiteSparse_long is always a + * 64-bit integer in a 64-bit code. ptrdiff_t might be a better choice than + * long; it is always the same size as a pointer. + * + * This file also defines the SUITESPARSE_VERSION and related definitions. + * + * Copyright (c) 2012, Timothy A. Davis. No licensing restrictions apply + * to this file or to the SuiteSparse_config directory. + * Author: Timothy A. Davis. + */ + +#ifndef SUITESPARSE_CONFIG_H +#define SUITESPARSE_CONFIG_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* ========================================================================== */ +/* === SuiteSparse_long ===================================================== */ +/* ========================================================================== */ + +#ifndef SuiteSparse_long + +#ifdef _WIN64 + +#define SuiteSparse_long __int64 +#define SuiteSparse_long_max _I64_MAX +#define SuiteSparse_long_idd "I64d" + +#else + +#define SuiteSparse_long long +#define SuiteSparse_long_max LONG_MAX +#define SuiteSparse_long_idd "ld" + +#endif +#define SuiteSparse_long_id "%" SuiteSparse_long_idd +#endif + +/* ========================================================================== */ +/* === SuiteSparse_config parameters and functions ========================== */ +/* ========================================================================== */ + +/* SuiteSparse-wide parameters are placed in this struct. It is meant to be + an extern, globally-accessible struct. It is not meant to be updated + frequently by multiple threads. Rather, if an application needs to modify + SuiteSparse_config, it should do it once at the beginning of the application, + before multiple threads are launched. + + The intent of these function pointers is that they not be used in your + application directly, except to assign them to the desired user-provided + functions. Rather, you should use the + */ + +struct SuiteSparse_config_struct +{ + void *(*malloc_func) (size_t) ; /* pointer to malloc */ + void *(*calloc_func) (size_t, size_t) ; /* pointer to calloc */ + void *(*realloc_func) (void *, size_t) ; /* pointer to realloc */ + void (*free_func) (void *) ; /* pointer to free */ + int (*printf_func) (const char *, ...) ; /* pointer to printf */ + double (*hypot_func) (double, double) ; /* pointer to hypot */ + int (*divcomplex_func) (double, double, double, double, double *, double *); +} ; + +extern struct SuiteSparse_config_struct SuiteSparse_config ; + +void SuiteSparse_start ( void ) ; /* called to start SuiteSparse */ + +void SuiteSparse_finish ( void ) ; /* called to finish SuiteSparse */ + +void *SuiteSparse_malloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to malloc (>=1 is enforced) */ + size_t size_of_item /* sizeof each item */ +) ; + +void *SuiteSparse_calloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to calloc (>=1 is enforced) */ + size_t size_of_item /* sizeof each item */ +) ; + +void *SuiteSparse_realloc /* pointer to reallocated block of memory, or + to original block if the realloc failed. */ +( + size_t nitems_new, /* new number of items in the object */ + size_t nitems_old, /* old number of items in the object */ + size_t size_of_item, /* sizeof each item */ + void *p, /* old object to reallocate */ + int *ok /* 1 if successful, 0 otherwise */ +) ; + +void *SuiteSparse_free /* always returns NULL */ +( + void *p /* block to free */ +) ; + +void SuiteSparse_tic /* start the timer */ +( + double tic [2] /* output, contents undefined on input */ +) ; + +double SuiteSparse_toc /* return time in seconds since last tic */ +( + double tic [2] /* input: from last call to SuiteSparse_tic */ +) ; + +double SuiteSparse_time /* returns current wall clock time in seconds */ +( + void +) ; + +/* returns sqrt (x^2 + y^2), computed reliably */ +double SuiteSparse_hypot (double x, double y) ; + +/* complex division of c = a/b */ +int SuiteSparse_divcomplex +( + double ar, double ai, /* real and imaginary parts of a */ + double br, double bi, /* real and imaginary parts of b */ + double *cr, double *ci /* real and imaginary parts of c */ +) ; + +/* determine which timer to use, if any */ +#ifndef NTIMER +#ifdef _POSIX_C_SOURCE +#if _POSIX_C_SOURCE >= 199309L +#define SUITESPARSE_TIMER_ENABLED +#endif +#endif +#endif + +/* SuiteSparse printf macro */ +#define SUITESPARSE_PRINTF(params) \ +{ \ + if (SuiteSparse_config.printf_func != NULL) \ + { \ + (void) (SuiteSparse_config.printf_func) params ; \ + } \ +} + +/* ========================================================================== */ +/* === SuiteSparse version ================================================== */ +/* ========================================================================== */ + +/* SuiteSparse is not a package itself, but a collection of packages, some of + * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD, + * COLAMD, CAMD, and CCOLAMD, etc). A version number is provided here for the + * collection itself. The versions of packages within each version of + * SuiteSparse are meant to work together. Combining one package from one + * version of SuiteSparse, with another package from another version of + * SuiteSparse, may or may not work. + * + * SuiteSparse contains the following packages: + * + * SuiteSparse_config version 4.5.6 (version always the same as SuiteSparse) + * AMD version 2.4.6 + * BTF version 1.2.6 + * CAMD version 2.4.6 + * CCOLAMD version 2.9.6 + * CHOLMOD version 3.0.11 + * COLAMD version 2.9.6 + * CSparse version 3.1.9 + * CXSparse version 3.1.9 + * GPUQREngine version 1.0.5 + * KLU version 1.3.8 + * LDL version 2.2.6 + * RBio version 2.2.6 + * SPQR version 2.0.8 + * SuiteSparse_GPURuntime version 1.0.5 + * UMFPACK version 5.7.6 + * MATLAB_Tools various packages & M-files + * xerbla version 1.0.3 + * + * Other package dependencies: + * BLAS required by CHOLMOD and UMFPACK + * LAPACK required by CHOLMOD + * METIS 5.1.0 required by CHOLMOD (optional) and KLU (optional) + * CUBLAS, CUDART NVIDIA libraries required by CHOLMOD and SPQR when + * they are compiled with GPU acceleration. + */ + +int SuiteSparse_version /* returns SUITESPARSE_VERSION */ +( + /* output, not defined on input. Not used if NULL. Returns + the three version codes in version [0..2]: + version [0] is SUITESPARSE_MAIN_VERSION + version [1] is SUITESPARSE_SUB_VERSION + version [2] is SUITESPARSE_SUBSUB_VERSION + */ + int version [3] +) ; + +/* Versions prior to 4.2.0 do not have the above function. The following + code fragment will work with any version of SuiteSparse: + + #ifdef SUITESPARSE_HAS_VERSION_FUNCTION + v = SuiteSparse_version (NULL) ; + #else + v = SUITESPARSE_VERSION ; + #endif +*/ +#define SUITESPARSE_HAS_VERSION_FUNCTION + +#define SUITESPARSE_DATE "Oct 3, 2017" +#define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) +#define SUITESPARSE_MAIN_VERSION 4 +#define SUITESPARSE_SUB_VERSION 5 +#define SUITESPARSE_SUBSUB_VERSION 6 +#define SUITESPARSE_VERSION \ + SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) + +#ifdef __cplusplus +} +#endif +#endif diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk new file mode 100644 index 000000000..2c13a6010 --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk @@ -0,0 +1,601 @@ +#=============================================================================== +# SuiteSparse_config.mk: common configuration file for the SuiteSparse +#=============================================================================== + +# This file contains all configuration settings for all packages in SuiteSparse, +# except for CSparse (which is stand-alone) and the packages in MATLAB_Tools. + +SUITESPARSE_VERSION = 4.5.6 + +#=============================================================================== +# Options you can change without editing this file: +#=============================================================================== + + # To list the options you can modify at the 'make' command line, type + # 'make config', which also lists their default values. You can then + # change them with 'make OPTION=value'. For example, to use an INSTALL + # path of /my/path, and to use your own BLAS and LAPACK libraries, do: + # + # make install INSTALL=/my/path BLAS=-lmyblas LAPACK=-lmylapackgoeshere + # + # which will install the package into /my/path/lib and /my/path/include, + # and use -lmyblas -lmylapackgoes here when building the demo program. + +#=============================================================================== +# Defaults for any system +#=============================================================================== + + #--------------------------------------------------------------------------- + # SuiteSparse root directory + #--------------------------------------------------------------------------- + + # Most Makefiles are in SuiteSparse/Pkg/Lib or SuiteSparse/Pkg/Demo, so + # the top-level of SuiteSparse is in ../.. unless otherwise specified. + # This is true for all but the SuiteSparse_config package. + SUITESPARSE ?= $(realpath $(CURDIR)/../..) + + #--------------------------------------------------------------------------- + # installation location + #--------------------------------------------------------------------------- + + # For "make install" and "make uninstall", the default location is + # SuiteSparse/lib, SuiteSparse/include, and + # SuiteSparse/share/doc/suitesparse-x.y.z + # If you do this: + # make install INSTALL=/usr/local + # then the libraries are installed in /usr/local/lib, include files in + # /usr/local/include, and documentation in + # /usr/local/share/doc/suitesparse-x.y.z. + # You can instead specify the install location of each of these 3 components + # separately, via (for example): + # make install INSTALL_LIB=/yada/mylibs INSTALL_INCLUDE=/yoda/myinc \ + # INSTALL_DOC=/solo/mydox + # which puts the libraries in /yada/mylibs, include files in /yoda/myinc, + # and documentation in /solo/mydox. + INSTALL ?= $(SUITESPARSE) + INSTALL_LIB ?= $(INSTALL)/lib + INSTALL_INCLUDE ?= $(INSTALL)/include + INSTALL_DOC ?= $(INSTALL)/share/doc/suitesparse-$(SUITESPARSE_VERSION) + + #--------------------------------------------------------------------------- + # optimization level + #--------------------------------------------------------------------------- + + OPTIMIZATION ?= -O3 + + #--------------------------------------------------------------------------- + # statement coverage for */Tcov + #--------------------------------------------------------------------------- + + ifeq ($(TCOV),yes) + # Each package has a */Tcov directory for extensive testing, including + # statement coverage. The Tcov tests require Linux and gcc, and use + # the vanilla BLAS. For those tests, the packages use 'make TCOV=yes', + # which overrides the following settings: + MKLROOT = + AUTOCC = no + CC = gcc + CXX = g++ + BLAS = -lrefblas -lgfortran -lstdc++ + LAPACK = -llapack + CFLAGS += --coverage + OPTIMIZATION = -g + LDFLAGS += --coverage + endif + + #--------------------------------------------------------------------------- + # CFLAGS for the C/C++ compiler + #--------------------------------------------------------------------------- + + # The CF macro is used by SuiteSparse Makefiles as a combination of + # CFLAGS, CPPFLAGS, TARGET_ARCH, and system-dependent settings. + CF ?= $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) $(OPTIMIZATION) -fexceptions -fPIC + + #--------------------------------------------------------------------------- + # OpenMP is used in CHOLMOD + #--------------------------------------------------------------------------- + + # with gcc, enable OpenMP directives via -fopenmp + # This is not supported on Darwin, so this string is cleared, below. + CFOPENMP ?= -fopenmp + + #--------------------------------------------------------------------------- + # compiler + #--------------------------------------------------------------------------- + + # By default, look for the Intel compilers. If present, they are used + # instead of $(CC), $(CXX), and $(F77). To disable this feature and + # use the $(CC), $(CXX), and $(F77) compilers, use 'make AUTOCC=no' + + AUTOCC ?= yes + + ifneq ($(AUTOCC),no) + ifneq ($(shell which icc 2>/dev/null),) + # use the Intel icc compiler for C codes, and -qopenmp for OpenMP + CC = icc -D_GNU_SOURCE + CXX = $(CC) + CFOPENMP = -qopenmp -I$(MKLROOT)/include + LDFLAGS += -openmp + endif + ifneq ($(shell which ifort 2>/dev/null),) + # use the Intel ifort compiler for Fortran codes + F77 = ifort + endif + endif + + #--------------------------------------------------------------------------- + # code formatting (for Tcov on Linux only) + #--------------------------------------------------------------------------- + + PRETTY ?= grep -v "^\#" | indent -bl -nce -bli0 -i4 -sob -l120 + + #--------------------------------------------------------------------------- + # required libraries + #--------------------------------------------------------------------------- + + # SuiteSparse requires the BLAS, LAPACK, and -lm (Math) libraries. + # It places its shared *.so libraries in SuiteSparse/lib. + # Linux also requires the -lrt library (see below) + LDLIBS ?= -lm + LDFLAGS += -L$(INSTALL_LIB) + + # See http://www.openblas.net for a recent and freely available optimzed + # BLAS. LAPACK is at http://www.netlib.org/lapack/ . You can use the + # standard Fortran LAPACK along with OpenBLAS to obtain very good + # performance. This script can also detect if the Intel MKL BLAS is + # installed. + + LAPACK ?= -llapack + + ifndef BLAS + ifdef MKLROOT + # use the Intel MKL for BLAS and LAPACK + # using static linking: + # BLAS = -Wl,--start-group \ + # $(MKLROOT)/lib/intel64/libmkl_intel_lp64.a \ + # $(MKLROOT)/lib/intel64/libmkl_core.a \ + # $(MKLROOT)/lib/intel64/libmkl_intel_thread.a \ + # -Wl,--end-group -lpthread -lm + # using dynamic linking: + BLAS = -lmkl_intel_lp64 -lmkl_core -lmkl_intel_thread -lpthread -lm + LAPACK = + else + # use the OpenBLAS at http://www.openblas.net + BLAS = -lopenblas + endif + endif + + # For ACML, use this instead: + # make BLAS='-lacml -lgfortran' + + #--------------------------------------------------------------------------- + # shell commands + #--------------------------------------------------------------------------- + + # ranlib, and ar, for generating libraries. If you don't need ranlib, + # just change it to RANLAB = echo + RANLIB ?= ranlib + ARCHIVE ?= $(AR) $(ARFLAGS) + CP ?= cp -f + MV ?= mv -f + + #--------------------------------------------------------------------------- + # Fortran compiler (not required for 'make' or 'make library') + #--------------------------------------------------------------------------- + + # A Fortran compiler is optional. Only required for the optional Fortran + # interfaces to AMD and UMFPACK. Not needed by 'make' or 'make install' + F77 ?= gfortran + F77FLAGS ?= $(FFLAGS) $(OPTIMIZATION) + + #--------------------------------------------------------------------------- + # NVIDIA CUDA configuration for CHOLMOD and SPQR + #--------------------------------------------------------------------------- + + # CUDA is detected automatically, and used if found. To disable CUDA, + # use CUDA=no + + ifneq ($(CUDA),no) + CUDA_PATH = $(shell which nvcc 2>/dev/null | sed "s/\/bin\/nvcc//") + endif + + ifeq ($(wildcard $(CUDA_PATH)),) + # CUDA is not present + CUDA_PATH = + GPU_BLAS_PATH = + GPU_CONFIG = + CUDART_LIB = + CUBLAS_LIB = + CUDA_INC_PATH = + CUDA_INC = + NVCC = echo + NVCCFLAGS = + else + # with CUDA for CHOLMOD and SPQR + GPU_BLAS_PATH = $(CUDA_PATH) + # GPU_CONFIG must include -DGPU_BLAS to compile SuiteSparse for the + # GPU. You can add additional GPU-related flags to it as well. + # with 4 cores (default): + GPU_CONFIG = -DGPU_BLAS + # For example, to compile CHOLMOD for 10 CPU cores when using the GPU: + # GPU_CONFIG = -DGPU_BLAS -DCHOLMOD_OMP_NUM_THREADS=10 + CUDART_LIB = $(CUDA_PATH)/lib64/libcudart.so + CUBLAS_LIB = $(CUDA_PATH)/lib64/libcublas.so + CUDA_INC_PATH = $(CUDA_PATH)/include/ + CUDA_INC = -I$(CUDA_INC_PATH) + NVCC = $(CUDA_PATH)/bin/nvcc + NVCCFLAGS = -Xcompiler -fPIC -O3 \ + -gencode=arch=compute_30,code=sm_30 \ + -gencode=arch=compute_35,code=sm_35 \ + -gencode=arch=compute_50,code=sm_50 \ + -gencode=arch=compute_50,code=compute_50 + endif + + #--------------------------------------------------------------------------- + # UMFPACK configuration: + #--------------------------------------------------------------------------- + + # Configuration for UMFPACK. See UMFPACK/Source/umf_config.h for details. + # + # -DNBLAS do not use the BLAS. UMFPACK will be very slow. + # -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by + # LAPACK and the BLAS (defaults to 'int') + # -DNSUNPERF do not use the Sun Perf. Library on Solaris + # -DNRECIPROCAL do not multiply by the reciprocal + # -DNO_DIVIDE_BY_ZERO do not divide by zero + # -DNCHOLMOD do not use CHOLMOD as a ordering method. If -DNCHOLMOD is + # included in UMFPACK_CONFIG, then UMFPACK does not rely on + # CHOLMOD, CAMD, CCOLAMD, COLAMD, and METIS. + + UMFPACK_CONFIG ?= + + # For example, uncomment this line to compile UMFPACK without CHOLMOD: + # UMFPACK_CONFIG = -DNCHOLMOD + # or use 'make UMFPACK_CONFIG=-DNCHOLMOD' + + #--------------------------------------------------------------------------- + # CHOLMOD configuration + #--------------------------------------------------------------------------- + + # CHOLMOD Library Modules, which appear in -lcholmod + # Core requires: none + # Check requires: Core + # Cholesky requires: Core, AMD, COLAMD. optional: Partition, Supernodal + # MatrixOps requires: Core + # Modify requires: Core + # Partition requires: Core, CCOLAMD, METIS. optional: Cholesky + # Supernodal requires: Core, BLAS, LAPACK + # + # CHOLMOD test/demo Modules (these do not appear in -lcholmod): + # Tcov requires: Core, Check, Cholesky, MatrixOps, Modify, Supernodal + # optional: Partition + # Valgrind same as Tcov + # Demo requires: Core, Check, Cholesky, MatrixOps, Supernodal + # optional: Partition + # + # Configuration flags: + # -DNCHECK do not include the Check module. + # -DNCHOLESKY do not include the Cholesky module. + # -DNPARTITION do not include the Partition module. + # also do not include METIS. + # -DNCAMD do not use CAMD & CCOLAMD in Parition Module. + # -DNMATRIXOPS do not include the MatrixOps module. + # -DNMODIFY do not include the Modify module. + # -DNSUPERNODAL do not include the Supernodal module. + # + # -DNPRINT do not print anything. + # -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by + # LAPACK and the BLAS (defaults to 'int') + # -DNSUNPERF for Solaris only. If defined, do not use the Sun + # Performance Library + # -DGPU_BLAS enable the use of the CUDA BLAS + + CHOLMOD_CONFIG ?= $(GPU_CONFIG) + + #--------------------------------------------------------------------------- + # SuiteSparseQR configuration: + #--------------------------------------------------------------------------- + + # The SuiteSparseQR library can be compiled with the following options: + # + # -DNPARTITION do not include the CHOLMOD partition module + # -DNEXPERT do not include the functions in SuiteSparseQR_expert.cpp + # -DHAVE_TBB enable the use of Intel's Threading Building Blocks + # -DGPU_BLAS enable the use of the CUDA BLAS + + SPQR_CONFIG ?= $(GPU_CONFIG) + + # to compile with Intel's TBB, use TBB=-ltbb -DSPQR_CONFIG=-DHAVE_TBB + TBB ?= + # TBB = -ltbb -DSPQR_CONFIG=-DHAVE_TBB + + # TODO: this *mk file should auto-detect the presence of Intel's TBB, + # and set the compiler flags accordingly. + +#=============================================================================== +# System-dependent configurations +#=============================================================================== + + #--------------------------------------------------------------------------- + # determine what system we are on + #--------------------------------------------------------------------------- + + # To disable these auto configurations, use 'make UNAME=custom' + + ifndef UNAME + ifeq ($(OS),Windows_NT) + # Cygwin Make on Windows has an $(OS) variable, but not uname. + # Note that this option is untested. + UNAME = Windows + else + # Linux and Darwin (Mac OSX) have been tested. + UNAME := $(shell uname) + endif + endif + + #--------------------------------------------------------------------------- + # Linux + #--------------------------------------------------------------------------- + + ifeq ($(UNAME),Linux) + # add the realtime library, librt, and SuiteSparse/lib + LDLIBS += -lrt -Wl,-rpath=$(INSTALL_LIB) + endif + + #--------------------------------------------------------------------------- + # Mac + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), Darwin) + # To compile on the Mac, you must install Xcode. Then do this at the + # command line in the Terminal, before doing 'make': + # xcode-select --install + CF += -fno-common + BLAS = -framework Accelerate + LAPACK = -framework Accelerate + # OpenMP is not yet supported by default in clang + CFOPENMP = + endif + + #--------------------------------------------------------------------------- + # Solaris + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), SunOS) + # Using the Sun compiler and the Sun Performance Library + # This hasn't been tested recently. + # I leave it here in case you need it. It likely needs updating. + CF += -fast -KPIC -xc99=%none -xlibmieee -xlibmil -m64 -Xc + F77FLAGS = -O -fast -KPIC -dalign -xlibmil -m64 + BLAS = -xlic_lib=sunperf + LAPACK = + # Using the GCC compiler and the reference BLAS + ## CC = gcc + ## CXX = g++ + ## MAKE = gmake + ## BLAS = -lrefblas -lgfortran + ## LAPACK = -llapack + endif + + #--------------------------------------------------------------------------- + # IBM AIX + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), AIX) + # hasn't been tested for a very long time... + # I leave it here in case you need it. It likely needs updating. + CF += -O4 -qipa -qmaxmem=16384 -q64 -qproto -DBLAS_NO_UNDERSCORE + F77FLAGS = -O4 -qipa -qmaxmem=16384 -q64 + BLAS = -lessl + LAPACK = + endif + +#=============================================================================== +# finalize the CF compiler flags +#=============================================================================== + + CF += $(CFOPENMP) + +#=============================================================================== +# internal configuration +#=============================================================================== + + # The user should not have to change these definitions, and they are + # not displayed by 'make config' + + #--------------------------------------------------------------------------- + # for removing files not in the distribution + #--------------------------------------------------------------------------- + + # remove object files, but keep compiled libraries via 'make clean' + CLEAN = *.o *.obj *.ln *.bb *.bbg *.da *.tcov *.gcov gmon.out *.bak *.d \ + *.gcda *.gcno *.aux *.bbl *.blg *.log *.toc *.dvi *.lof *.lot + + # also remove compiled libraries, via 'make distclean' + PURGE = *.so* *.a *.dll *.dylib *.dSYM + + # location of TCOV test output + TCOV_TMP ?= /tmp + +#=============================================================================== +# Building the shared and static libraries +#=============================================================================== + +# How to build/install shared and static libraries for Mac and Linux/Unix. +# This assumes that LIBRARY and VERSION have already been defined by the +# Makefile that includes this file. + +SO_OPTS = $(LDFLAGS) + +ifeq ($(UNAME),Windows) + # Cygwin Make on Windows (untested) + AR_TARGET = $(LIBRARY).lib + SO_PLAIN = $(LIBRARY).dll + SO_MAIN = $(LIBRARY).$(SO_VERSION).dll + SO_TARGET = $(LIBRARY).$(VERSION).dll + SO_INSTALL_NAME = echo +else + # Mac or Linux/Unix + AR_TARGET = $(LIBRARY).a + ifeq ($(UNAME),Darwin) + # Mac + SO_PLAIN = $(LIBRARY).dylib + SO_MAIN = $(LIBRARY).$(SO_VERSION).dylib + SO_TARGET = $(LIBRARY).$(VERSION).dylib + SO_OPTS += -dynamiclib -compatibility_version $(SO_VERSION) \ + -current_version $(VERSION) \ + -shared -undefined dynamic_lookup + # When a Mac *.dylib file is moved, this command is required + # to change its internal name to match its location in the filesystem: + SO_INSTALL_NAME = install_name_tool -id + else + # Linux and other variants of Unix + SO_PLAIN = $(LIBRARY).so + SO_MAIN = $(LIBRARY).so.$(SO_VERSION) + SO_TARGET = $(LIBRARY).so.$(VERSION) + SO_OPTS += -shared -Wl,-soname -Wl,$(SO_MAIN) -Wl,--no-undefined + # Linux/Unix *.so files can be moved without modification: + SO_INSTALL_NAME = echo + endif +endif + +#=============================================================================== +# Configure CHOLMOD/Partition module with METIS, CAMD, and CCOLAMD +#=============================================================================== + +# By default, SuiteSparse uses METIS 5.1.0 in the SuiteSparse/metis-5.1.0 +# directory. SuiteSparse's interface to METIS is only through the +# SuiteSparse/CHOLMOD/Partition module, which also requires SuiteSparse/CAMD +# and SuiteSparse/CCOLAMD. +# +# If you wish to use your own pre-installed copy of METIS, use the MY_METIS_LIB +# and MY_METIS_INC options passed to 'make'. For example: +# make MY_METIS_LIB=-lmetis +# make MY_METIS_LIB=/home/myself/mylibraries/libmetis.so +# make MY_METIS_LIB='-L/home/myself/mylibraries -lmetis' +# If you need to tell the compiler where to find the metis.h include file, +# then add MY_METIS_INC=/home/myself/metis-5.1.0/include as well, which points +# to the directory containing metis.h. If metis.h is already installed in +# a location known to the compiler (/usr/local/include/metis.h for example) +# then you do not need to add MY_METIS_INC. + +I_WITH_PARTITION = +LIB_WITH_PARTITION = +CONFIG_PARTITION = -DNPARTITION -DNCAMD +# check if CAMD/CCOLAMD and METIS are requested and available +ifeq (,$(findstring -DNCAMD, $(CHOLMOD_CONFIG))) + # CAMD and CCOLAMD are requested. See if they are available in + # SuiteSparse/CAMD and SuiteSparse/CCOLAMD + ifneq (, $(wildcard $(SUITESPARSE)/CAMD)) + ifneq (, $(wildcard $(SUITESPARSE)/CCOLAMD)) + # CAMD and CCOLAMD are requested and available + LIB_WITH_PARTITION = -lccolamd -lcamd + I_WITH_PARTITION = -I$(SUITESPARSE)/CCOLAMD/Include -I$(SUITESPARSE)/CAMD/Include + CONFIG_PARTITION = -DNPARTITION + # check if METIS is requested and available + ifeq (,$(findstring -DNPARTITION, $(CHOLMOD_CONFIG))) + # METIS is requested. See if it is available. + ifneq (,$(MY_METIS_LIB)) + # METIS 5.1.0 is provided elsewhere, and we are not using + # SuiteSparse/metis-5.1.0. To do so, we link with + # $(MY_METIS_LIB) and add the -I$(MY_METIS_INC) option for + # the compiler. The latter can be empty if you have METIS + # installed in a place where the compiler can find the + # metis.h include file by itself without any -I option + # (/usr/local/include/metis.h for example). + LIB_WITH_PARTITION += $(MY_METIS_LIB) + ifneq (,$(MY_METIS_INC)) + I_WITH_PARTITION += -I$(MY_METIS_INC) + endif + CONFIG_PARTITION = + else + # see if METIS is in SuiteSparse/metis-5.1.0 + ifneq (, $(wildcard $(SUITESPARSE)/metis-5.1.0)) + # SuiteSparse/metis5.1.0 is available + ifeq ($(UNAME), Darwin) + LIB_WITH_PARTITION += $(SUITESPARSE)/lib/libmetis.dylib + else + LIB_WITH_PARTITION += -lmetis + endif + I_WITH_PARTITION += -I$(SUITESPARSE)/metis-5.1.0/include + CONFIG_PARTITION = + endif + endif + endif + endif + endif +endif + +#=============================================================================== +# display configuration +#=============================================================================== + +ifeq ($(LIBRARY),) + # placeholders, for 'make config' in the top-level SuiteSparse + LIBRARY=PackageNameWillGoHere + VERSION=x.y.z + SO_VERSION=x +endif + +# 'make config' lists the primary installation options +config: + @echo ' ' + @echo '----------------------------------------------------------------' + @echo 'SuiteSparse package compilation options:' + @echo '----------------------------------------------------------------' + @echo ' ' + @echo 'SuiteSparse Version: ' '$(SUITESPARSE_VERSION)' + @echo 'SuiteSparse top folder: ' '$(SUITESPARSE)' + @echo 'Package: LIBRARY= ' '$(LIBRARY)' + @echo 'Version: VERSION= ' '$(VERSION)' + @echo 'SO version: SO_VERSION= ' '$(SO_VERSION)' + @echo 'System: UNAME= ' '$(UNAME)' + @echo 'Install directory: INSTALL= ' '$(INSTALL)' + @echo 'Install libraries in: INSTALL_LIB= ' '$(INSTALL_LIB)' + @echo 'Install include files in: INSTALL_INCLUDE=' '$(INSTALL_INCLUDE)' + @echo 'Install documentation in: INSTALL_DOC= ' '$(INSTALL_DOC)' + @echo 'Optimization level: OPTIMIZATION= ' '$(OPTIMIZATION)' + @echo 'BLAS library: BLAS= ' '$(BLAS)' + @echo 'LAPACK library: LAPACK= ' '$(LAPACK)' + @echo 'Intel TBB library: TBB= ' '$(TBB)' + @echo 'Other libraries: LDLIBS= ' '$(LDLIBS)' + @echo 'static library: AR_TARGET= ' '$(AR_TARGET)' + @echo 'shared library (full): SO_TARGET= ' '$(SO_TARGET)' + @echo 'shared library (main): SO_MAIN= ' '$(SO_MAIN)' + @echo 'shared library (short): SO_PLAIN= ' '$(SO_PLAIN)' + @echo 'shared library options: SO_OPTS= ' '$(SO_OPTS)' + @echo 'shared library name tool: SO_INSTALL_NAME=' '$(SO_INSTALL_NAME)' + @echo 'ranlib, for static libs: RANLIB= ' '$(RANLIB)' + @echo 'static library command: ARCHIVE= ' '$(ARCHIVE)' + @echo 'copy file: CP= ' '$(CP)' + @echo 'move file: MV= ' '$(MV)' + @echo 'remove file: RM= ' '$(RM)' + @echo 'pretty (for Tcov tests): PRETTY= ' '$(PRETTY)' + @echo 'C compiler: CC= ' '$(CC)' + @echo 'C++ compiler: CXX= ' '$(CXX)' + @echo 'CUDA compiler: NVCC= ' '$(NVCC)' + @echo 'CUDA root directory: CUDA_PATH= ' '$(CUDA_PATH)' + @echo 'OpenMP flags: CFOPENMP= ' '$(CFOPENMP)' + @echo 'C/C++ compiler flags: CF= ' '$(CF)' + @echo 'LD flags: LDFLAGS= ' '$(LDFLAGS)' + @echo 'Fortran compiler: F77= ' '$(F77)' + @echo 'Fortran flags: F77FLAGS= ' '$(F77FLAGS)' + @echo 'Intel MKL root: MKLROOT= ' '$(MKLROOT)' + @echo 'Auto detect Intel icc: AUTOCC= ' '$(AUTOCC)' + @echo 'UMFPACK config: UMFPACK_CONFIG= ' '$(UMFPACK_CONFIG)' + @echo 'CHOLMOD config: CHOLMOD_CONFIG= ' '$(CHOLMOD_CONFIG)' + @echo 'SuiteSparseQR config: SPQR_CONFIG= ' '$(SPQR_CONFIG)' + @echo 'CUDA library: CUDART_LIB= ' '$(CUDART_LIB)' + @echo 'CUBLAS library: CUBLAS_LIB= ' '$(CUBLAS_LIB)' + @echo 'METIS and CHOLMOD/Partition configuration:' + @echo 'Your METIS library: MY_METIS_LIB= ' '$(MY_METIS_LIB)' + @echo 'Your metis.h is in: MY_METIS_INC= ' '$(MY_METIS_INC)' + @echo 'METIS is used via the CHOLMOD/Partition module, configured as follows.' + @echo 'If the next line has -DNPARTITION then METIS will not be used:' + @echo 'CHOLMOD Partition config: ' '$(CONFIG_PARTITION)' + @echo 'CHOLMOD Partition libs: ' '$(LIB_WITH_PARTITION)' + @echo 'CHOLMOD Partition include:' '$(I_WITH_PARTITION)' +ifeq ($(TCOV),yes) + @echo 'TCOV=yes, for extensive testing only (gcc, g++, vanilla BLAS)' +endif + diff --git a/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile b/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile new file mode 100644 index 000000000..db68a2ea8 --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile @@ -0,0 +1,75 @@ +# Makefile for null-output xerbla, both C and Fortran versions. +# By default, the C version (libcerbla.a and *.so) is compiled and installed. +# Set the USE_FORTRAN option to 1 to create the Fortran instead (libxerbla): + USE_FORTRAN = 0 +# USE_FORTRAN = 1 + +VERSION = 1.0.3 +SO_VERSION = 1 + +default: library + +# compile and install in SuiteSparse/lib +library: + $(MAKE) install INSTALL=$(SUITESPARSE) + +all: library + +ifeq ($(USE_FORTRAN),0) + LIBRARY = libcerbla +else + LIBRARY = libxerbla +endif + +include ../SuiteSparse_config.mk + +ifeq ($(USE_FORTRAN),0) + COMPILE = $(CC) $(CF) -c xerbla.c + DEPENDS = xerbla.c xerbla.h +else + COMPILE = $(F77) $(F77FLAGS) -c xerbla.f + DEPENDS = xerbla.f +endif + +ccode: all + +fortran: all + +static: $(AR_TARGET) + +$(AR_TARGET): $(DEPENDS) + $(COMPILE) + $(ARCHIVE) $(AR_TARGET) xerbla.o + - $(RANLIB) $(AR_TARGET) + - $(RM) xerbla.o + +# install libcerbla / libxerbla +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(DEPENDS) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(COMPILE) + $(CC) $(SO_OPTS) xerbla.o -o $@ + - $(RM) xerbla.o + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) xerbla.h $(INSTALL_INCLUDE) + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 644 $(INSTALL_INCLUDE)/xerbla.h + +# uninstall libcerbla / libxerbla +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_INCLUDE)/xerbla.h + +distclean: purge + +purge: clean + - $(RM) -r $(PURGE) + +clean: + - $(RM) -r $(CLEAN) + diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.c b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.c similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.c rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.c diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.f b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.f similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.f rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.f diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.h b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.h similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.h rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.h diff --git a/gtsam/3rdparty/UFconfig/README.txt b/gtsam/3rdparty/UFconfig/README.txt deleted file mode 100644 index d9edc239a..000000000 --- a/gtsam/3rdparty/UFconfig/README.txt +++ /dev/null @@ -1,35 +0,0 @@ -UFconfig contains configuration settings for all many of the software packages -that I develop or co-author. Note that older versions of some of these packages -do not require UFconfig. - - Package Description - ------- ----------- - AMD approximate minimum degree ordering - CAMD constrained AMD - COLAMD column approximate minimum degree ordering - CCOLAMD constrained approximate minimum degree ordering - UMFPACK sparse LU factorization, with the BLAS - CXSparse int/long/real/complex version of CSparse - CHOLMOD sparse Cholesky factorization, update/downdate - KLU sparse LU factorization, BLAS-free - BTF permutation to block triangular form - LDL concise sparse LDL' - LPDASA LP Dual Active Set Algorithm - SuiteSparseQR sparse QR factorization - -UFconfig is not required by: - - CSparse a Concise Sparse matrix package - RBio read/write files in Rutherford/Boeing format - UFcollection tools for managing the UF Sparse Matrix Collection - LINFACTOR simple m-file to show how to use LU and CHOL to solve Ax=b - MESHND 2D and 3D mesh generation and nested dissection ordering - MATLAB_Tools misc collection of m-files - SSMULT sparse matrix times sparse matrix, for use in MATLAB - -In addition, the xerbla/ directory contains Fortan and C versions of the -BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to -the BLAS or LAPACK. The xerbla provided here does not print any message, so -the entire Fortran I/O library does not need to be linked into a C application. -Most versions of the BLAS contain xerbla, but those from K. Goto do not. Use -this if you need too. diff --git a/gtsam/3rdparty/UFconfig/UFconfig.c b/gtsam/3rdparty/UFconfig/UFconfig.c deleted file mode 100644 index 8b8d45ef7..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.c +++ /dev/null @@ -1,71 +0,0 @@ -/* ========================================================================== */ -/* === UFconfig ============================================================= */ -/* ========================================================================== */ - -/* Copyright (c) 2009, University of Florida. No licensing restrictions - * apply to this file or to the UFconfig directory. Author: Timothy A. Davis. - */ - -#include "UFconfig.h" - -/* -------------------------------------------------------------------------- */ -/* UFmalloc: malloc wrapper */ -/* -------------------------------------------------------------------------- */ - -void *UFmalloc /* pointer to allocated block of memory */ -( - size_t nitems, /* number of items to malloc (>=1 is enforced) */ - size_t size_of_item, /* sizeof each item */ - int *ok, /* TRUE if successful, FALSE otherwise */ - UFconfig *config /* SuiteSparse-wide configuration */ -) -{ - void *p ; - if (nitems < 1) nitems = 1 ; - if (nitems * size_of_item != ((double) nitems) * size_of_item) - { - /* Int overflow */ - *ok = 0 ; - return (NULL) ; - } - if (!config || config->malloc_memory == NULL) - { - /* use malloc by default */ - p = (void *) malloc (nitems * size_of_item) ; - } - else - { - /* use the pointer to malloc in the config */ - p = (void *) (config->malloc_memory) (nitems * size_of_item) ; - } - *ok = (p != NULL) ; - return (p) ; -} - - -/* -------------------------------------------------------------------------- */ -/* UFfree: free wrapper */ -/* -------------------------------------------------------------------------- */ - -void *UFfree /* always returns NULL */ -( - void *p, /* block to free */ - UFconfig *config /* SuiteSparse-wide configuration */ -) -{ - if (p) - { - if (!config || config->free_memory == NULL) - { - /* use free by default */ - free (p) ; - } - else - { - /* use the pointer to free in the config */ - (config->free_memory) (p) ; - } - } - return (NULL) ; -} - diff --git a/gtsam/3rdparty/UFconfig/UFconfig.h b/gtsam/3rdparty/UFconfig/UFconfig.h deleted file mode 100644 index 795f5668c..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.h +++ /dev/null @@ -1,152 +0,0 @@ -/* ========================================================================== */ -/* === UFconfig.h =========================================================== */ -/* ========================================================================== */ - -/* Configuration file for SuiteSparse: a Suite of Sparse matrix packages - * (AMD, COLAMD, CCOLAMD, CAMD, CHOLMOD, UMFPACK, CXSparse, and others). - * - * UFconfig.h provides the definition of the long integer. On most systems, - * a C program can be compiled in LP64 mode, in which long's and pointers are - * both 64-bits, and int's are 32-bits. Windows 64, however, uses the LLP64 - * model, in which int's and long's are 32-bits, and long long's and pointers - * are 64-bits. - * - * SuiteSparse packages that include long integer versions are - * intended for the LP64 mode. However, as a workaround for Windows 64 - * (and perhaps other systems), the long integer can be redefined. - * - * If _WIN64 is defined, then the __int64 type is used instead of long. - * - * The long integer can also be defined at compile time. For example, this - * could be added to UFconfig.mk: - * - * CFLAGS = -O -D'UF_long=long long' -D'UF_long_max=9223372036854775801' \ - * -D'UF_long_idd="lld"' - * - * This file defines UF_long as either long (on all but _WIN64) or - * __int64 on Windows 64. The intent is that a UF_long is always a 64-bit - * integer in a 64-bit code. ptrdiff_t might be a better choice than long; - * it is always the same size as a pointer. - * - * This file also defines the SUITESPARSE_VERSION and related definitions. - * - * Copyright (c) 2007, University of Florida. No licensing restrictions - * apply to this file or to the UFconfig directory. Author: Timothy A. Davis. - */ - -#ifndef _UFCONFIG_H -#define _UFCONFIG_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -/* ========================================================================== */ -/* === UF_long ============================================================== */ -/* ========================================================================== */ - -#ifndef UF_long - -#ifdef _WIN64 - -#define UF_long __int64 -#define UF_long_max _I64_MAX -#define UF_long_idd "I64d" - -#else - -#define UF_long long -#define UF_long_max LONG_MAX -#define UF_long_idd "ld" - -#endif -#define UF_long_id "%" UF_long_idd -#endif - -/* ========================================================================== */ -/* === UFconfig parameters and functions ==================================== */ -/* ========================================================================== */ - -/* SuiteSparse-wide parameters will be placed in this struct. So far, they - are only used by RBio. */ - -typedef struct UFconfig_struct -{ - void *(*malloc_memory) (size_t) ; /* pointer to malloc */ - void *(*realloc_memory) (void *, size_t) ; /* pointer to realloc */ - void (*free_memory) (void *) ; /* pointer to free */ - void *(*calloc_memory) (size_t, size_t) ; /* pointer to calloc */ - -} UFconfig ; - -void *UFmalloc /* pointer to allocated block of memory */ -( - size_t nitems, /* number of items to malloc (>=1 is enforced) */ - size_t size_of_item, /* sizeof each item */ - int *ok, /* TRUE if successful, FALSE otherwise */ - UFconfig *config /* SuiteSparse-wide configuration */ -) ; - -void *UFfree /* always returns NULL */ -( - void *p, /* block to free */ - UFconfig *config /* SuiteSparse-wide configuration */ -) ; - - -/* ========================================================================== */ -/* === SuiteSparse version ================================================== */ -/* ========================================================================== */ - -/* SuiteSparse is not a package itself, but a collection of packages, some of - * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD, - * COLAMD, CAMD, and CCOLAMD, etc). A version number is provided here for the - * collection itself. The versions of packages within each version of - * SuiteSparse are meant to work together. Combining one packge from one - * version of SuiteSparse, with another package from another version of - * SuiteSparse, may or may not work. - * - * SuiteSparse Version 3.6.1 contains the following packages: - * - * AMD version 2.2.2 - * BTF version 1.1.2 - * CAMD version 2.2.2 - * CCOLAMD version 2.7.3 - * CHOLMOD version 1.7.3 - * COLAMD version 2.7.3 - * CSparse version 2.2.5 - * CSparse3 version 3.0.1 - * CXSparse version 2.2.5 - * KLU version 1.1.2 - * LDL version 2.0.3 - * RBio version 2.0.1 - * SPQR version 1.2.2 (also called SuiteSparseQR) - * UFcollection version 1.5.0 - * UFconfig version number is the same as SuiteSparse - * UMFPACK version 5.5.1 - * LINFACTOR version 1.1.0 - * MESHND version 1.1.1 - * SSMULT version 2.0.2 - * MATLAB_Tools no specific version number - * - * Other package dependencies: - * BLAS required by CHOLMOD and UMFPACK - * LAPACK required by CHOLMOD - * METIS 4.0.1 required by CHOLMOD (optional) and KLU (optional) - */ - -#define SUITESPARSE_DATE "May 10, 2011" -#define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) -#define SUITESPARSE_MAIN_VERSION 3 -#define SUITESPARSE_SUB_VERSION 6 -#define SUITESPARSE_SUBSUB_VERSION 1 -#define SUITESPARSE_VERSION \ - SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) - -#ifdef __cplusplus -} -#endif -#endif diff --git a/gtsam/3rdparty/UFconfig/UFconfig.mk b/gtsam/3rdparty/UFconfig/UFconfig.mk deleted file mode 100644 index 60c951b6a..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.mk +++ /dev/null @@ -1,386 +0,0 @@ -#=============================================================================== -# UFconfig.mk: common configuration file for the SuiteSparse -#=============================================================================== - -# This file contains all configuration settings for all packages authored or -# co-authored by Tim Davis at the University of Florida: -# -# Package Version Description -# ------- ------- ----------- -# AMD 1.2 or later approximate minimum degree ordering -# COLAMD 2.4 or later column approximate minimum degree ordering -# CCOLAMD 1.0 or later constrained column approximate minimum degree ordering -# CAMD any constrained approximate minimum degree ordering -# UMFPACK 4.5 or later sparse LU factorization, with the BLAS -# CHOLMOD any sparse Cholesky factorization, update/downdate -# KLU 0.8 or later sparse LU factorization, BLAS-free -# BTF 0.8 or later permutation to block triangular form -# LDL 1.2 or later concise sparse LDL' -# LPDASA any linear program solve (dual active set algorithm) -# CXSparse any extended version of CSparse (int/long, real/complex) -# SuiteSparseQR any sparse QR factorization -# -# The UFconfig directory and the above packages should all appear in a single -# directory, in order for the Makefile's within each package to find this file. -# -# To enable an option of the form "# OPTION = ...", edit this file and -# delete the "#" in the first column of the option you wish to use. - -#------------------------------------------------------------------------------ -# Generic configuration -#------------------------------------------------------------------------------ - -# C compiler and compiler flags: These will normally not give you optimal -# performance. You should select the optimization parameters that are best -# for your system. On Linux, use "CFLAGS = -O3 -fexceptions" for example. -CC = cc -CFLAGS = -O3 -fexceptions - -# C++ compiler (also uses CFLAGS) -CPLUSPLUS = g++ - -# ranlib, and ar, for generating libraries -RANLIB = ranlib -AR = ar cr - -# copy, delete, and rename a file -CP = cp -f -RM = rm -f -MV = mv -f - -# Fortran compiler (not normally required) -F77 = f77 -F77FLAGS = -O -F77LIB = - -# C and Fortran libraries -LIB = -lm - -# For compiling MATLAB mexFunctions (MATLAB 7.5 or later) -MEX = mex -O -largeArrayDims -lmwlapack -lmwblas - -# For compiling MATLAB mexFunctions (MATLAB 7.3 and 7.4) -# MEX = mex -O -largeArrayDims -lmwlapack - -# For MATLAB 7.2 or earlier, you must use one of these options: -# MEX = mex -O -lmwlapack -# MEX = mex -O - -# Which version of MAKE you are using (default is "make") -# MAKE = make -# MAKE = gmake - -# For "make install" -INSTALL_LIB = /usr/local/lib -INSTALL_INCLUDE = /usr/local/include - -#------------------------------------------------------------------------------ -# BLAS and LAPACK configuration: -#------------------------------------------------------------------------------ - -# UMFPACK and CHOLMOD both require the BLAS. CHOLMOD also requires LAPACK. -# See Kazushige Goto's BLAS at http://www.cs.utexas.edu/users/flame/goto/ or -# http://www.tacc.utexas.edu/~kgoto/ for the best BLAS to use with CHOLMOD. -# LAPACK is at http://www.netlib.org/lapack/ . You can use the standard -# Fortran LAPACK along with Goto's BLAS to obtain very good performance. -# CHOLMOD gets a peak numeric factorization rate of 3.6 Gflops on a 3.2 GHz -# Pentium 4 (512K cache, 4GB main memory) with the Goto BLAS, and 6 Gflops -# on a 2.5Ghz dual-core AMD Opteron. - -# These settings will probably not work, since there is no fixed convention for -# naming the BLAS and LAPACK library (*.a or *.so) files. - -# This is probably slow ... it might connect to the Standard Reference BLAS: -BLAS = -lblas -lgfortran -LAPACK = -llapack - -# NOTE: this next option for the "Goto BLAS" has nothing to do with a "goto" -# statement. Rather, the Goto BLAS is written by Dr. Kazushige Goto. -# Using the Goto BLAS: -# BLAS = -lgoto -lgfortran -lgfortranbegin - -# Using non-optimized versions: -# BLAS = -lblas_plain -lgfortran -lgfortranbegin -# LAPACK = -llapack_plain - -# BLAS = -lblas_plain -lgfortran -lgfortranbegin -# LAPACK = -llapack - -# The BLAS might not contain xerbla, an error-handling routine for LAPACK and -# the BLAS. Also, the standard xerbla requires the Fortran I/O library, and -# stops the application program if an error occurs. A C version of xerbla -# distributed with this software (UFconfig/xerbla/libcerbla.a) includes a -# Fortran-callable xerbla routine that prints nothing and does not stop the -# application program. This is optional. -# XERBLA = ../../UFconfig/xerbla/libcerbla.a - -# If you wish to use the XERBLA in LAPACK and/or the BLAS instead, -# use this option: -XERBLA = - -# If you wish to use the Fortran UFconfig/xerbla/xerbla.f instead, use this: -# XERBLA = ../../UFconfig/xerbla/libxerbla.a - -#------------------------------------------------------------------------------ -# METIS, optionally used by CHOLMOD -#------------------------------------------------------------------------------ - -# If you do not have METIS, or do not wish to use it in CHOLMOD, you must -# compile CHOLMOD with the -DNPARTITION flag. You must also use the -# "METIS =" option, below. - -# The path is relative to where it is used, in CHOLMOD/Lib, CHOLMOD/MATLAB, etc. -# You may wish to use an absolute path. METIS is optional. Compile -# CHOLMOD with -DNPARTITION if you do not wish to use METIS. -METIS_PATH = ../../metis-4.0 -METIS = ../../metis-4.0/libmetis.a - -# If you use CHOLMOD_CONFIG = -DNPARTITION then you must use the following -# options: -# METIS_PATH = -# METIS = - -#------------------------------------------------------------------------------ -# UMFPACK configuration: -#------------------------------------------------------------------------------ - -# Configuration flags for UMFPACK. See UMFPACK/Source/umf_config.h for details. -# -# -DNBLAS do not use the BLAS. UMFPACK will be very slow. -# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by -# LAPACK and the BLAS (defaults to 'int') -# -DNSUNPERF do not use the Sun Perf. Library (default is use it on Solaris) -# -DNPOSIX do not use POSIX routines sysconf and times. -# -DGETRUSAGE use getrusage -# -DNO_TIMER do not use any timing routines -# -DNRECIPROCAL do not multiply by the reciprocal -# -DNO_DIVIDE_BY_ZERO do not divide by zero - -UMFPACK_CONFIG = - -#------------------------------------------------------------------------------ -# CHOLMOD configuration -#------------------------------------------------------------------------------ - -# CHOLMOD Library Modules, which appear in libcholmod.a: -# Core requires: none -# Check requires: Core -# Cholesky requires: Core, AMD, COLAMD. optional: Partition, Supernodal -# MatrixOps requires: Core -# Modify requires: Core -# Partition requires: Core, CCOLAMD, METIS. optional: Cholesky -# Supernodal requires: Core, BLAS, LAPACK -# -# CHOLMOD test/demo Modules (all are GNU GPL, do not appear in libcholmod.a): -# Tcov requires: Core, Check, Cholesky, MatrixOps, Modify, Supernodal -# optional: Partition -# Valgrind same as Tcov -# Demo requires: Core, Check, Cholesky, MatrixOps, Supernodal -# optional: Partition -# -# Configuration flags: -# -DNCHECK do not include the Check module. License GNU LGPL -# -DNCHOLESKY do not include the Cholesky module. License GNU LGPL -# -DNPARTITION do not include the Partition module. License GNU LGPL -# also do not include METIS. -# -DNGPL do not include any GNU GPL Modules in the CHOLMOD library: -# -DNMATRIXOPS do not include the MatrixOps module. License GNU GPL -# -DNMODIFY do not include the Modify module. License GNU GPL -# -DNSUPERNODAL do not include the Supernodal module. License GNU GPL -# -# -DNPRINT do not print anything. -# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by -# LAPACK and the BLAS (defaults to 'int') -# -DNSUNPERF for Solaris only. If defined, do not use the Sun -# Performance Library - -CHOLMOD_CONFIG = - -#------------------------------------------------------------------------------ -# SuiteSparseQR configuration: -#------------------------------------------------------------------------------ - -# The SuiteSparseQR library can be compiled with the following options: -# -# -DNPARTITION do not include the CHOLMOD partition module -# -DNEXPERT do not include the functions in SuiteSparseQR_expert.cpp -# -DTIMING enable timing and flop counts -# -DHAVE_TBB enable the use of Intel's Threading Building Blocks (TBB) - -# default, without timing, without TBB: -SPQR_CONFIG = -# with timing and TBB: -# SPQR_CONFIG = -DTIMING -DHAVE_TBB -# with timing -# SPQR_CONFIG = -DTIMING - -# This is needed for IBM AIX: (but not for and C codes, just C++) -# SPQR_CONFIG = -DBLAS_NO_UNDERSCORE - -# with TBB, you must select this: -# TBB = -ltbb -# without TBB: -TBB = - -# with timing, you must include the timing library: -# RTLIB = -lrt -# without timing -RTLIB = - -#------------------------------------------------------------------------------ -# Linux -#------------------------------------------------------------------------------ - -# Using default compilers: -# CC = gcc -# CFLAGS = -O3 -fexceptions - -# alternatives: -# CFLAGS = -g -fexceptions \ - -Wall -W -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ - -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi \ - -funit-at-a-time -# CFLAGS = -O3 -fexceptions \ - -Wall -W -Werror -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ - -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi -# CFLAGS = -O3 -fexceptions -D_FILE_OFFSET_BITS=64 -D_LARGEFILE64_SOURCE -# CFLAGS = -O3 -# CFLAGS = -O3 -g -fexceptions -# CFLAGS = -g -fexceptions \ - -Wall -W -Wshadow \ - -Wredundant-decls -Wdisabled-optimization -ansi - -# consider: -# -fforce-addr -fmove-all-movables -freduce-all-givs -ftsp-ordering -# -frename-registers -ffast-math -funroll-loops - -# Using the Goto BLAS: -# BLAS = -lgoto -lfrtbegin -lg2c $(XERBLA) -lpthread - -# Using Intel's icc and ifort compilers: -# (does not work for mexFunctions unless you add a mexopts.sh file) -# F77 = ifort -# CC = icc -# CFLAGS = -O3 -xN -vec_report=0 -# CFLAGS = -g -# old (broken): CFLAGS = -ansi -O3 -ip -tpp7 -xW -vec_report0 - -# 64bit: -# F77FLAGS = -O -m64 -# CFLAGS = -O3 -fexceptions -m64 -# BLAS = -lgoto64 -lfrtbegin -lg2c -lpthread $(XERBLA) -# LAPACK = -llapack64 - - -# SUSE Linux 10.1, AMD Opteron, with GOTO Blas -# F77 = gfortran -# BLAS = -lgoto_opteron64 -lgfortran - -# SUSE Linux 10.1, Intel Pentium, with GOTO Blas -# F77 = gfortran -# BLAS = -lgoto -lgfortran - -#------------------------------------------------------------------------------ -# Mac -#------------------------------------------------------------------------------ - -# As recommended by macports, http://suitesparse.darwinports.com/ -# I've tested them myself on Mac OSX 10.6.1 (Snow Leopard), on my MacBook Air. -# F77 = gfortran -# CFLAGS = -O3 -fno-common -no-cpp-precomp -fexceptions -# BLAS = -framework Accelerate -# LAPACK = -framework Accelerate - -# Using netlib.org LAPACK and BLAS compiled by gfortran, with and without -# optimzation: -# BLAS = -lblas_plain -lgfortran -# LAPACK = -llapack_plain -# BLAS = -lblas_optimized -lgfortran -# LAPACK = -llapack_optimized - -#------------------------------------------------------------------------------ -# Solaris -#------------------------------------------------------------------------------ - -# 32-bit -# CFLAGS = -KPIC -dalign -xc99=%none -Xc -xlibmieee -xO5 -xlibmil -m32 - -# 64-bit -# CFLAGS = -fast -KPIC -xc99=%none -xlibmieee -xlibmil -m64 -Xc - -# FFLAGS = -fast -KPIC -dalign -xlibmil -m64 - -# The Sun Performance Library includes both LAPACK and the BLAS: -# BLAS = -xlic_lib=sunperf -# LAPACK = - - -#------------------------------------------------------------------------------ -# Compaq Alpha -#------------------------------------------------------------------------------ - -# 64-bit mode only -# CFLAGS = -O2 -std1 -# BLAS = -ldxml -# LAPACK = - -#------------------------------------------------------------------------------ -# Macintosh -#------------------------------------------------------------------------------ - -# CC = gcc -# CFLAGS = -O3 -fno-common -no-cpp-precomp -fexceptions -# LIB = -lstdc++ -# BLAS = -framework Accelerate -# LAPACK = -framework Accelerate - -#------------------------------------------------------------------------------ -# IBM RS 6000 -#------------------------------------------------------------------------------ - -# BLAS = -lessl -# LAPACK = - -# 32-bit mode: -# CFLAGS = -O4 -qipa -qmaxmem=16384 -qproto -# F77FLAGS = -O4 -qipa -qmaxmem=16384 - -# 64-bit mode: -# CFLAGS = -O4 -qipa -qmaxmem=16384 -q64 -qproto -# F77FLAGS = -O4 -qipa -qmaxmem=16384 -q64 -# AR = ar -X64 - -#------------------------------------------------------------------------------ -# SGI IRIX -#------------------------------------------------------------------------------ - -# BLAS = -lscsl -# LAPACK = - -# 32-bit mode -# CFLAGS = -O - -# 64-bit mode (32 bit int's and 64-bit long's): -# CFLAGS = -64 -# F77FLAGS = -64 - -# SGI doesn't have ranlib -# RANLIB = echo - -#------------------------------------------------------------------------------ -# AMD Opteron (64 bit) -#------------------------------------------------------------------------------ - -# BLAS = -lgoto_opteron64 -lg2c -# LAPACK = -llapack_opteron64 - -# SUSE Linux 10.1, AMD Opteron -# F77 = gfortran -# BLAS = -lgoto_opteron64 -lgfortran -# LAPACK = -llapack_opteron64 - -#------------------------------------------------------------------------------ -# remove object files and profile output -#------------------------------------------------------------------------------ - -CLEAN = *.o *.obj *.ln *.bb *.bbg *.da *.tcov *.gcov gmon.out *.bak *.d *.gcda *.gcno diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index d3229427e..859f61513 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -25,8 +25,7 @@ add_subdirectory(3rdparty) set (3rdparty_srcs ${eigen_headers} # Set by 3rdparty/CMakeLists.txt ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c - ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c - ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/UFconfig/UFconfig.c) + ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config/SuiteSparse_config.c) gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure # To exclude a source from the library build (in any subfolder) @@ -136,6 +135,13 @@ else() set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) endif() +# make sure that ccolamd compiles even in face of warnings +if(WIN32) + set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w") +else() + set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error") +endif() + # Special cases if(MSVC) set_property(SOURCE diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp index 454dea45b..bdf0543ff 100644 --- a/gtsam/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -18,7 +18,6 @@ #include #include -#include #include using namespace std; @@ -79,7 +78,7 @@ DSFVector::DSFVector(const boost::shared_ptr& v_in, /* ************************************************************************* */ bool DSFVector::isSingleton(const size_t& label) const { bool result = false; - BOOST_FOREACH(size_t key,keys_) { + for(size_t key: keys_) { if (find(key) == label) { if (!result) // find the first occurrence result = true; @@ -93,7 +92,7 @@ bool DSFVector::isSingleton(const size_t& label) const { /* ************************************************************************* */ std::set DSFVector::set(const size_t& label) const { std::set < size_t > set; - BOOST_FOREACH(size_t key,keys_) + for(size_t key: keys_) if (find(key) == label) set.insert(key); return set; @@ -102,7 +101,7 @@ std::set DSFVector::set(const size_t& label) const { /* ************************************************************************* */ std::map > DSFVector::sets() const { std::map > sets; - BOOST_FOREACH(size_t key,keys_) + for(size_t key: keys_) sets[find(key)].insert(key); return sets; } @@ -110,7 +109,7 @@ std::map > DSFVector::sets() const { /* ************************************************************************* */ std::map > DSFVector::arrays() const { std::map > arrays; - BOOST_FOREACH(size_t key,keys_) + for(size_t key: keys_) arrays[find(key)].push_back(key); return arrays; } diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h index 3c91d68c5..d73accf11 100644 --- a/gtsam/base/DSFVector.h +++ b/gtsam/base/DSFVector.h @@ -13,13 +13,16 @@ * @file DSFVector.h * @date Jun 25, 2010 * @author Kai Ni - * @brief A faster implementation for DSF, which uses vector rather than btree. As a result, the size of the forest is prefixed. + * @brief A faster implementation for DSF, which uses vector rather than btree. */ #pragma once +#include #include + #include + #include #include #include @@ -41,27 +44,26 @@ private: boost::shared_ptr v_;///< Stores parent pointers, representative iff v[i]==i public: - /// constructor that allocate new memory, allows for keys 0...numNodes-1 + /// Constructor that allocates new memory, allows for keys 0...numNodes-1. DSFBase(const size_t numNodes); - /// constructor that uses the existing memory + /// Constructor that uses an existing, pre-allocated vector. DSFBase(const boost::shared_ptr& v_in); - /// find the label of the set in which {key} lives + /// Find the label of the set in which {key} lives. size_t find(size_t key) const; - /// Merge two sets + /// Merge the sets containing i1 and i2. Does nothing if i1 and i2 are already in the same set. void merge(const size_t& i1, const size_t& i2); - /// @deprecated old name +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 inline size_t findSet(size_t key) const {return find(key);} - - /// @deprecated old name inline void makeUnionInPlace(const size_t& i1, const size_t& i2) {return merge(i1,i2);} +#endif }; /** - * DSFVector additionaly keeps a vector of keys to support more expensive operations + * DSFVector additionally keeps a vector of keys to support more expensive operations * @addtogroup base */ class GTSAM_EXPORT DSFVector: public DSFBase { @@ -70,27 +72,27 @@ private: std::vector keys_; ///< stores keys to support more expensive operations public: - /// constructor that allocate new memory, uses sequential keys 0...numNodes-1 + /// Constructor that allocates new memory, uses sequential keys 0...numNodes-1. DSFVector(const size_t numNodes); - /// constructor that allocates memory, uses given keys + /// Constructor that allocates memory, uses given keys. DSFVector(const std::vector& keys); - /// constructor that uses the existing memory + /// Constructor that uses existing vectors. DSFVector(const boost::shared_ptr& v_in, const std::vector& keys); - // all operations below loop over all keys and hence are *at least* O(n) + // All operations below loop over all keys and hence are *at least* O(n) - /// find whether there is one and only one occurrence for the given {label} + /// Find whether there is one and only one occurrence for the given {label}. bool isSingleton(const size_t& label) const; - /// get the nodes in the tree with the given label + /// Get the nodes in the tree with the given label std::set set(const size_t& label) const; - /// return all sets, i.e. a partition of all elements + /// Return all sets, i.e. a partition of all elements. std::map > sets() const; - /// return all sets, i.e. a partition of all elements + /// Return all sets, i.e. a partition of all elements. std::map > arrays() const; }; diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 17783c5b9..93a7d0db5 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include // operator typeid namespace gtsam { @@ -190,7 +190,7 @@ public: } /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues -#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(GenericValue) +#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam::GenericValue) }; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index a9d521a7f..0c37b405e 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -33,6 +32,7 @@ #include #include #include +#include using namespace std; @@ -190,7 +190,7 @@ istream& operator>>(istream& inputStream, Matrix& destinationMatrix) { // Copy coefficients to matrix destinationMatrix.resize(height, width); int row = 0; - BOOST_FOREACH(const vector& rowVec, coeffs) { + for(const vector& rowVec: coeffs) { destinationMatrix.row(row) = Eigen::Map(&rowVec[0], width); ++ row; } @@ -419,7 +419,7 @@ Matrix stack(size_t nrMatrices, ...) Matrix stack(const std::vector& blocks) { if (blocks.size() == 1) return blocks.at(0); DenseIndex nrows = 0, ncols = blocks.at(0).cols(); - BOOST_FOREACH(const Matrix& mat, blocks) { + for(const Matrix& mat: blocks) { nrows += mat.rows(); if (ncols != mat.cols()) throw invalid_argument("Matrix::stack(): column size mismatch!"); @@ -427,7 +427,7 @@ Matrix stack(const std::vector& blocks) { Matrix result(nrows, ncols); DenseIndex cur_row = 0; - BOOST_FOREACH(const Matrix& mat, blocks) { + for(const Matrix& mat: blocks) { result.middleRows(cur_row, mat.rows()) = mat; cur_row += mat.rows(); } @@ -441,7 +441,7 @@ Matrix collect(const std::vector& matrices, size_t m, size_t n) size_t dimA1 = m; size_t dimA2 = n*matrices.size(); if (!m && !n) { - BOOST_FOREACH(const Matrix* M, matrices) { + for(const Matrix* M: matrices) { dimA1 = M->rows(); // TODO: should check if all the same ! dimA2 += M->cols(); } @@ -450,7 +450,7 @@ Matrix collect(const std::vector& matrices, size_t m, size_t n) // stl::copy version Matrix A(dimA1, dimA2); size_t hindex = 0; - BOOST_FOREACH(const Matrix* M, matrices) { + for(const Matrix* M: matrices) { size_t row_len = M->cols(); A.block(0, hindex, dimA1, row_len) = *M; hindex += row_len; @@ -611,7 +611,7 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, boost::tokenizer > tok(matrixStr, boost::char_separator("\n")); DenseIndex row = 0; - BOOST_FOREACH(const std::string& line, tok) + for(const std::string& line: tok) { assert(row < effectiveRows); if(row > 0) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 463b5f5d9..87ead88f0 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -138,17 +138,27 @@ public: return ProductLieGroup(g,h); } static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet"); - G g = traits::Expmap(v.template head()); - H h = traits::Expmap(v.template tail()); + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); + H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); + if (Hv) { + Hv->setZero(); + Hv->template topLeftCorner() = D_g_first; + Hv->template bottomRightCorner() = D_h_second; + } return ProductLieGroup(g,h); } static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { - if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet"); - typename traits::TangentVector v1 = traits::Logmap(p.first); - typename traits::TangentVector v2 = traits::Logmap(p.second); + Jacobian1 D_g_first; Jacobian2 D_h_second; + typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? &D_g_first : 0); + typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? &D_h_second : 0); TangentVector v; v << v1, v2; + if (Hp) { + Hp->setZero(); + Hp->template topLeftCorner() = D_g_first; + Hp->template bottomRightCorner() = D_h_second; + } return v; } ProductLieGroup expmap(const TangentVector& v) const { diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 7cca63092..5eaa2df1c 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -51,32 +51,42 @@ SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf( } /* ************************************************************************* */ -VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( - DenseIndex nFrontals) { - // Do dense elimination - if (blockStart() != 0) - throw std::invalid_argument( - "Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0."); - if (!gtsam::choleskyPartial(matrix_, offset(nFrontals))) +Matrix SymmetricBlockMatrix::block(DenseIndex I, DenseIndex J) const { + if (I == J) { + return diagonalBlock(I); + } else if (I < J) { + return aboveDiagonalBlock(I, J); + } else { + return aboveDiagonalBlock(J, I).transpose(); + } +} + +/* ************************************************************************* */ +void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) { + gttic(VerticalBlockMatrix_choleskyPartial); + DenseIndex topleft = variableColOffsets_[blockStart_]; + if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft)) throw CholeskyFailed(); +} - // Split conditional +/* ************************************************************************* */ +VerticalBlockMatrix SymmetricBlockMatrix::split(DenseIndex nFrontals) { + gttic(VerticalBlockMatrix_split); - // Create one big conditionals with many frontal variables. - gttic(Construct_conditional); - const size_t varDim = offset(nFrontals); - VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); - Ab.full() = matrix_.topRows(varDim); - Ab.full().triangularView().setZero(); - gttoc(Construct_conditional); + // Construct a VerticalBlockMatrix that contains [R Sd] + const size_t n1 = offset(nFrontals); + VerticalBlockMatrix RSd = VerticalBlockMatrix::LikeActiveViewOf(*this, n1); + + // Copy into it. + RSd.full() = matrix_.topRows(n1); + RSd.full().triangularView().setZero(); - gttic(Remaining_factor); // Take lower-right block of Ab_ to get the remaining factor blockStart() = nFrontals; - gttoc(Remaining_factor); - return Ab; + return RSd; } + /* ************************************************************************* */ } //\ namespace gtsam diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 1f81ca1f9..53a8912f6 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- -* GTSAM Copyright 2010, Georgia Tech Research Corporation, +* 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) @@ -19,13 +19,12 @@ #include #include -#include #include #include #include #include #include -#include +#include namespace boost { namespace serialization { @@ -53,8 +52,8 @@ namespace gtsam { { public: typedef SymmetricBlockMatrix This; - typedef SymmetricBlockMatrixBlockExpr Block; - typedef SymmetricBlockMatrixBlockExpr constBlock; + typedef Eigen::Block Block; + typedef Eigen::Block constBlock; protected: Matrix matrix_; ///< The full matrix @@ -105,12 +104,12 @@ namespace gtsam { throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix."); assertInvariants(); } - + /// Copy the block structure, but do not copy the matrix data. If blockStart() has been /// modified, this copies the structure of the corresponding matrix view. In the destination /// SymmetricBlockMatrix, blockStart() will be 0. static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other); - + /// Copy the block structure, but do not copy the matrix data. If blockStart() has been /// modified, this copies the structure of the corresponding matrix view. In the destination /// SymmetricBlockMatrix, blockStart() will be 0. @@ -123,71 +122,165 @@ namespace gtsam { DenseIndex cols() const { return rows(); } /// Block count - DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } + DenseIndex nBlocks() const { return nActualBlocks() - blockStart_; } - /// Access the block with vertical block index \c i_block and horizontal block index \c j_block. - /// Note that the actual block accessed in the underlying matrix is relative to blockStart(). - Block operator()(DenseIndex i_block, DenseIndex j_block) { - return Block(*this, i_block, j_block); + /// Number of dimensions for variable on this diagonal block. + DenseIndex getDim(DenseIndex block) const { + return calcIndices(block, block, 1, 1)[2]; } - /// Access the block with vertical block index \c i_block and horizontal block index \c j_block. - /// Note that the actual block accessed in the underlying matrix is relative to blockStart(). - constBlock operator()(DenseIndex i_block, DenseIndex j_block) const { - return constBlock(*this, i_block, j_block); + /// @name Block getter methods. + /// @{ + + /// Get a copy of a block (anywhere in the matrix). + /// This method makes a copy - use the methods below if performance is critical. + Matrix block(DenseIndex I, DenseIndex J) const; + + /// Return the J'th diagonal block as a self adjoint view. + Eigen::SelfAdjointView diagonalBlock(DenseIndex J) { + return block_(J, J).selfadjointView(); } - /// Access the range of blocks starting with vertical block index \c i_startBlock, ending with - /// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock, - /// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note - /// that the actual blocks accessed in the underlying matrix are relative to blockStart(). - Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) { - assertInvariants(); - return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock); + /// Return the J'th diagonal block as a self adjoint view. + Eigen::SelfAdjointView diagonalBlock(DenseIndex J) const { + return block_(J, J).selfadjointView(); } - /// Access the range of blocks starting with vertical block index \c i_startBlock, ending with - /// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock, - /// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note - /// that the actual blocks accessed in the underlying matrix are relative to blockStart(). - constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const { - assertInvariants(); - return constBlock(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock); + /// Get the diagonal of the J'th diagonal block. + Vector diagonal(DenseIndex J) const { + return block_(J, J).diagonal(); } - /** Return the full matrix, *not* including any portions excluded by firstBlock(). */ - Block full() - { - return Block(*this, 0, nBlocks(), 0); + /// Get block above the diagonal (I, J). + constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const { + assert(I < J); + return block_(I, J); } - /** Return the full matrix, *not* including any portions excluded by firstBlock(). */ - constBlock full() const - { - return constBlock(*this, 0, nBlocks(), 0); + /// Return the square sub-matrix that contains blocks(i:j, i:j). + Eigen::SelfAdjointView selfadjointView( + DenseIndex I, DenseIndex J) const { + assert(J > I); + return block_(I, I, J - I, J - I).selfadjointView(); } - /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ - Eigen::SelfAdjointView matrix() const - { - return matrix_; + /// Return the square sub-matrix that contains blocks(i:j, i:j) as a triangular view. + Eigen::TriangularView triangularView(DenseIndex I, + DenseIndex J) const { + assert(J > I); + return block_(I, I, J - I, J - I).triangularView(); } - /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ - Eigen::SelfAdjointView matrix() - { - return matrix_; + /// Get a range [i,j) from the matrix. Indices are in block units. + constBlock aboveDiagonalRange(DenseIndex i_startBlock, + DenseIndex i_endBlock, + DenseIndex j_startBlock, + DenseIndex j_endBlock) const { + assert(i_startBlock < j_startBlock); + assert(i_endBlock <= j_startBlock); + return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock, + j_endBlock - j_startBlock); } - /// Return the absolute offset in the underlying matrix of the start of the specified \c block. - DenseIndex offset(DenseIndex block) const - { - assertInvariants(); - DenseIndex actualBlock = block + blockStart_; - checkBlock(actualBlock); - return variableColOffsets_[actualBlock]; + /// Get a range [i,j) from the matrix. Indices are in block units. + Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, + DenseIndex j_startBlock, DenseIndex j_endBlock) { + assert(i_startBlock < j_startBlock); + assert(i_endBlock <= j_startBlock); + return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock, + j_endBlock - j_startBlock); } + /// @} + /// @name Block setter methods. + /// @{ + + /// Set a diagonal block. Only the upper triangular portion of `xpr` is evaluated. + template + void setDiagonalBlock(DenseIndex I, const XprType& xpr) { + block_(I, I).triangularView() = xpr.template triangularView(); + } + + /// Set an off-diagonal block. Only the upper triangular portion of `xpr` is evaluated. + template + void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) { + assert(I != J); + if (I < J) { + block_(I, J) = xpr; + } else { + block_(J, I) = xpr.transpose(); + } + } + + /// Increment the diagonal block by the values in `xpr`. Only reads the upper triangular part of `xpr`. + template + void updateDiagonalBlock(DenseIndex I, const XprType& xpr) { + // TODO(gareth): Eigen won't let us add triangular or self-adjoint views + // here, so we do it manually. + auto dest = block_(I, I); + assert(dest.rows() == xpr.rows()); + assert(dest.cols() == xpr.cols()); + for (DenseIndex col = 0; col < dest.cols(); ++col) { + for (DenseIndex row = 0; row <= col; ++row) { + dest(row, col) += xpr(row, col); + } + } + } + + /// Update an off diagonal block. + /// NOTE(emmett): This assumes noalias(). + template + void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) { + assert(I != J); + if (I < J) { + block_(I, J).noalias() += xpr; + } else { + block_(J, I).noalias() += xpr.transpose(); + } + } + + /// @} + /// @name Accessing the full matrix. + /// @{ + + /// Get self adjoint view. + Eigen::SelfAdjointView selfadjointView() { + return full().selfadjointView(); + } + + /// Get self adjoint view. + Eigen::SelfAdjointView selfadjointView() const { + return full().selfadjointView(); + } + + /// Set the entire active matrix. Only reads the upper triangular part of `xpr`. + template + void setFullMatrix(const XprType& xpr) { + full().triangularView() = xpr.template triangularView(); + } + + /// Set the entire active matrix zero. + void setZero() { + full().triangularView().setZero(); + } + + /// Negate the entire active matrix. + void negate() { + full().triangularView() *= -1.0; + } + + /// Invert the entire active matrix in place. + void invertInPlace() { + const auto identity = Matrix::Identity(rows(), rows()); + full().triangularView() = + selfadjointView() + .llt() + .solve(identity) + .triangularView(); + } + + /// @} + /// Retrieve or modify the first logical block, i.e. the block referenced by block index 0. /// Blocks before it will be inaccessible, except by accessing the underlying matrix using /// matrix(). @@ -197,11 +290,86 @@ namespace gtsam { /// it will be inaccessible, except by accessing the underlying matrix using matrix(). DenseIndex blockStart() const { return blockStart_; } - /// Do partial Cholesky in-place and return the eliminated block matrix, leaving the remaining - /// symmetric matrix in place. - VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals); + /** + * Given the augmented Hessian [A1'A1 A1'A2 A1'b + * A2'A1 A2'A2 A2'b + * b'A1 b'A2 b'b] + * on x1 and x2, does partial Cholesky in-place to obtain [R Sd;0 L] such that + * R'R = A1'A1 + * R'Sd = [A1'A2 A1'b] + * L'L is the augmented Hessian on the the separator x2 + * R and Sd can be interpreted as a GaussianConditional |R*x1 + S*x2 - d]^2 + */ + void choleskyPartial(DenseIndex nFrontals); + + /** + * After partial Cholesky, we can optionally split off R and Sd, to be interpreted as + * a GaussianConditional |R*x1 + S*x2 - d]^2. We leave the symmetric lower block L in place, + * and adjust block_start so now *this refers to it. + */ + VerticalBlockMatrix split(DenseIndex nFrontals); protected: + + /// Number of offsets in the full matrix. + DenseIndex nOffsets() const { + return variableColOffsets_.size(); + } + + /// Number of actual blocks in the full matrix. + DenseIndex nActualBlocks() const { + return nOffsets() - 1; + } + + /// Get an offset for a block index (in the active view). + DenseIndex offset(DenseIndex block) const { + assert(block >= 0); + const DenseIndex actual_index = block + blockStart(); + assert(actual_index < nOffsets()); + return variableColOffsets_[actual_index]; + } + + /// Get an arbitrary block from the matrix. Indices are in block units. + constBlock block_(DenseIndex iBlock, DenseIndex jBlock, + DenseIndex blockRows = 1, DenseIndex blockCols = 1) const { + const std::array indices = + calcIndices(iBlock, jBlock, blockRows, blockCols); + return matrix_.block(indices[0], indices[1], indices[2], indices[3]); + } + + /// Get an arbitrary block from the matrix. Indices are in block units. + Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows = 1, + DenseIndex blockCols = 1) { + const std::array indices = + calcIndices(iBlock, jBlock, blockRows, blockCols); + return matrix_.block(indices[0], indices[1], indices[2], indices[3]); + } + + /// Get the full matrix as a block. + constBlock full() const { + return block_(0, 0, nBlocks(), nBlocks()); + } + + /// Get the full matrix as a block. + Block full() { + return block_(0, 0, nBlocks(), nBlocks()); + } + + /// Compute the indices into the underlying matrix for a given block. + std::array calcIndices(DenseIndex iBlock, DenseIndex jBlock, + DenseIndex blockRows, + DenseIndex blockCols) const { + assert(blockRows >= 0); + assert(blockCols >= 0); + + // adjust indices to account for start and size of blocks + const DenseIndex denseI = offset(iBlock); + const DenseIndex denseJ = offset(jBlock); + const DenseIndex denseRows = offset(iBlock + blockRows) - denseI; + const DenseIndex denseCols = offset(jBlock + blockCols) - denseJ; + return {{denseI, denseJ, denseRows, denseCols}}; + } + void assertInvariants() const { assert(matrix_.rows() == matrix_.cols()); @@ -209,21 +377,6 @@ namespace gtsam { assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); } - void checkBlock(DenseIndex block) const - { - static_cast(block); //Disable unused varibale warnings. - assert(matrix_.rows() == matrix_.cols()); - assert(matrix_.cols() == variableColOffsets_.back()); - assert(block >= 0); - assert(block < (DenseIndex)variableColOffsets_.size()-1); - assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); - } - - DenseIndex offsetUnchecked(DenseIndex block) const - { - return variableColOffsets_[block + blockStart_]; - } - template void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { diff --git a/gtsam/base/SymmetricBlockMatrixBlockExpr.h b/gtsam/base/SymmetricBlockMatrixBlockExpr.h deleted file mode 100644 index dd999ae6c..000000000 --- a/gtsam/base/SymmetricBlockMatrixBlockExpr.h +++ /dev/null @@ -1,337 +0,0 @@ -/* ---------------------------------------------------------------------------- - -* 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 SymmetricBlockMatrixBlockExpr.h -* @brief Matrix expression for a block of a SymmetricBlockMatrix -* @author Richard Roberts -* @date Nov 20, 2013 -*/ -#pragma once - -#include - -namespace gtsam { template class SymmetricBlockMatrixBlockExpr; } -namespace gtsam { class SymmetricBlockMatrix; } - -// traits class for Eigen expressions -namespace Eigen -{ - namespace internal - { - template - struct traits > : - public traits::type> - { - }; - } -} - -namespace gtsam -{ - /// A matrix expression that references a single block of a SymmetricBlockMatrix. Depending on - /// the position of the block, this expression will behave either as a regular matrix block, a - /// transposed matrix block, or a symmetric matrix block. The only reason this class is templated - /// on SymmetricBlockMatrixType is to allow for both const and non-const references. - template - class SymmetricBlockMatrixBlockExpr : public Eigen::EigenBase > - { - protected: - SymmetricBlockMatrixType& xpr_; ///< The referenced SymmetricBlockMatrix - DenseIndex densei_; ///< The scalar indices of the referenced block - DenseIndex densej_; ///< The scalar indices of the referenced block - DenseIndex denseRows_; ///< The scalar size of the referenced block - DenseIndex denseCols_; ///< The scalar size of the referenced block - enum BlockType { Plain, SelfAdjoint, Transposed } blockType_; ///< The type of the referenced block, as determined by the block position - typedef SymmetricBlockMatrixBlockExpr This; - - public: - // Typedefs and constants used in Eigen - typedef typename const_selector::Scalar&, typename Eigen::internal::traits::Scalar>::type ScalarRef; - typedef typename Eigen::internal::traits::Scalar Scalar; - typedef typename Eigen::internal::traits::Index Index; - static const Index ColsAtCompileTime = Eigen::Dynamic; - static const Index RowsAtCompileTime = Eigen::Dynamic; - - typedef typename const_selector::type - DenseMatrixType; - - typedef Eigen::Map > OffDiagonal; - typedef Eigen::SelfAdjointView, Eigen::Upper> SelfAdjointView; - typedef Eigen::TriangularView, Eigen::Upper> TriangularView; - - protected: - mutable Eigen::Block myBlock_; - template friend class SymmetricBlockMatrixBlockExpr; - - public: - /// Create a SymmetricBlockMatrixBlockExpr from the specified block of a SymmetricBlockMatrix. - SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index iBlock, Index jBlock) : - xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) - { - initIndices(iBlock, jBlock); - } - - /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a - /// SymmetricBlockMatrix. - SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, - Index firstRowBlock, Index firstColBlock, Index rowBlocks, Index colBlocks) : - xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) - { - initIndices(firstRowBlock, firstColBlock, rowBlocks, colBlocks); - } - - /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a - /// SymmetricBlockMatrix. - SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char /*dummy*/) : - xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) - { - initIndices(firstBlock, firstBlock, blocks, blocks); - } - - inline Index rows() const { return blockType_ != Transposed ? denseRows_ : denseCols_; } - inline Index cols() const { return blockType_ != Transposed ? denseCols_ : denseRows_; } - - inline BlockType blockType() const { return blockType_; } - - inline ScalarRef operator()(Index row, Index col) const - { - return coeffInternal(row, col); - } - - inline OffDiagonal knownOffDiagonal() const - { - typedef Eigen::Stride DynamicStride; - - // We can return a Map if we are either on an off-diagonal block, or a block of size 0 or 1 - assert(blockType_ != SelfAdjoint || (denseRows_ <= 1 && denseCols_ <= 1)); - if(blockType_ == Transposed) - { - // Swap the inner and outer stride to produce a transposed Map - Eigen::Block block = const_cast(*this).xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_); - return Eigen::Map(block.data(), block.cols(), block.rows(), - DynamicStride(block.innerStride(), block.outerStride())); - } - else - { - Eigen::Block block = const_cast(*this).xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_); - return Eigen::Map(block.data(), block.rows(), block.cols(), - DynamicStride(block.outerStride(), block.innerStride())); - } - } - - inline SelfAdjointView selfadjointView() const - { - assert(blockType_ == SelfAdjoint); - return myBlock_; - } - - inline TriangularView triangularView() const - { - assert(blockType_ == SelfAdjoint); - return myBlock_; - } - - template inline void evalTo(Dest& dst) const - { - // Just try to assign to the object using either a selfadjoint view or a block view - if(blockType_ == SelfAdjoint) - dst = selfadjointView(); - else if(blockType_ == Plain) - dst = myBlock_; - else - dst = myBlock_.transpose(); - } - - //template inline void evalTo(const Eigen::SelfAdjointView& rhs) const - //{ - // if(blockType_ == SelfAdjoint) - // rhs.nestedExpression().triangularView() = triangularView(); - // else - // throw std::invalid_argument("Cannot assign an off-diagonal block to a self-adjoint matrix"); - //} - - //template inline void evalTo(const Eigen::TriangularView& rhs) const - //{ - // if(blockType_ == SelfAdjoint) - // rhs.nestedExpression().triangularView() = triangularView(); - // else - // throw std::invalid_argument("Cannot assign an off-diagonal block to a self-adjoint matrix"); - //} - - template - This& operator=(const Eigen::MatrixBase& rhs) - { - // Just try to assign to the object using either a selfadjoint view or a block view - if(blockType_ == SelfAdjoint) - triangularView() = rhs.derived().template triangularView(); - else if(blockType_ == Plain) - myBlock_ = rhs.derived(); - else - myBlock_.transpose() = rhs.derived(); - return *this; - } - - template - This& operator=(const Eigen::SelfAdjointView& rhs) - { - if(blockType_ == SelfAdjoint) - triangularView() = rhs.nestedExpression().template triangularView(); - else - throw std::invalid_argument("Cannot assign a self-adjoint matrix to an off-diagonal block"); - return *this; - } - - template - This& operator=(const SymmetricBlockMatrixBlockExpr& other) - { - _doAssign(other); - return *this; - } - - This& operator=(const This& other) - { - // This version is required so GCC doesn't synthesize a default operator=. - _doAssign(other); - return *this; - } - - template - This& operator+=(const SymmetricBlockMatrixBlockExpr& other) - { - if(blockType_ == SelfAdjoint) - { - assert((BlockType)other.blockType() == SelfAdjoint); - triangularView() += other.triangularView().nestedExpression(); - } - else if(blockType_ == Plain) - { - assert((BlockType)other.blockType() == Plain || (BlockType)other.blockType() == Transposed); - if((BlockType)other.blockType() == Transposed) - myBlock_ += other.myBlock_.transpose(); - else - myBlock_ += other.myBlock_; - } - else - { - assert((BlockType)other.blockType() == Plain || (BlockType)other.blockType() == Transposed); - if((BlockType)other.blockType() == Transposed) - myBlock_.transpose() += other.myBlock_.transpose(); - else - myBlock_.transpose() += other.myBlock_; - } - return *this; - } - - private: - void initIndices(Index iBlock, Index jBlock, Index blockRows = 1, Index blockCols = 1) - { - if(iBlock == jBlock && blockRows == blockCols) - { - densei_ = xpr_.offset(iBlock); - densej_ = densei_; - if(blockRows > 0) - xpr_.checkBlock(iBlock + blockRows - 1); - denseRows_ = xpr_.offsetUnchecked(iBlock + blockRows) - densei_; - if(blockCols > 0) - xpr_.checkBlock(jBlock + blockCols - 1); - denseCols_ = xpr_.offsetUnchecked(jBlock + blockCols) - densej_; - blockType_ = SelfAdjoint; - } - else - { - if(jBlock > iBlock || (iBlock == jBlock && blockCols > blockRows)) - { - densei_ = xpr_.offset(iBlock); - densej_ = xpr_.offset(jBlock); - if(blockRows > 0) - xpr_.checkBlock(iBlock + blockRows - 1); - denseRows_ = xpr_.offsetUnchecked(iBlock + blockRows) - densei_; - if(blockCols > 0) - xpr_.checkBlock(jBlock + blockCols - 1); - denseCols_ = xpr_.offsetUnchecked(jBlock + blockCols) - densej_; - blockType_ = Plain; - } - else - { - densei_ = xpr_.offset(jBlock); - densej_ = xpr_.offset(iBlock); - if(blockCols > 0) - xpr_.checkBlock(jBlock + blockCols - 1); - denseRows_ = xpr_.offsetUnchecked(jBlock + blockCols) - densei_; - if(blockRows > 0) - xpr_.checkBlock(iBlock + blockRows - 1); - denseCols_ = xpr_.offsetUnchecked(iBlock + blockRows) - densej_; - blockType_ = Transposed; - } - - // Validate that the block does not cross below the diagonal (the indices have already been - // flipped above the diagonal for ranges starting below the diagonal). - if(densei_ + denseRows_ > densej_ + 1) - throw std::invalid_argument("Off-diagonal block ranges may not cross the diagonal"); - } - - new (&myBlock_) Eigen::Block(xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_)); - } - - template - inline ScalarType coeffInternal(Index row, Index col) const - { - // We leave index checking up to the Block class - if(blockType_ == Plain) - { - return myBlock_(row, col); - } - else if(blockType_ == SelfAdjoint) - { - if(row <= col) - return myBlock_(row, col); - else - return myBlock_.transpose()(row, col); - } - else - { - return myBlock_.transpose()(row, col); - } - } - - template - void _doAssign(const SymmetricBlockMatrixBlockExpr& other) - { - if(blockType_ == SelfAdjoint) - { - assert((BlockType)other.blockType() == SelfAdjoint); - triangularView() = other.triangularView().nestedExpression(); - } - else if(blockType_ == Plain) - { - assert((BlockType)other.blockType() == Plain || (BlockType)other.blockType() == Transposed); - if((BlockType)other.blockType() == Transposed) - myBlock_ = other.myBlock_.transpose(); - else - myBlock_ = other.myBlock_; - } - else - { - assert((BlockType)other.blockType() == Plain || (BlockType)other.blockType() == Transposed); - if((BlockType)other.blockType() == Transposed) - myBlock_.transpose() = other.myBlock_.transpose(); - else - myBlock_.transpose() = other.myBlock_; - } - } - - - }; - -} diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index f976be0e7..8792be0e6 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -20,7 +20,6 @@ #include #include -#include #include #include #include @@ -91,7 +90,7 @@ bool assert_equal(const std::vector& expected, const std::vector& actual, match = false; if(match) { size_t i = 0; - BOOST_FOREACH(const V& a, expected) { + for(const V& a: expected) { if (!assert_equal(a, actual[i++], tol)) { match = false; break; @@ -100,9 +99,9 @@ bool assert_equal(const std::vector& expected, const std::vector& actual, } if(!match) { std::cout << "expected: " << std::endl; - BOOST_FOREACH(const V& a, expected) { std::cout << a << " "; } + for(const V& a: expected) { std::cout << a << " "; } std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const V& a, actual) { std::cout << a << " "; } + for(const V& a: actual) { std::cout << a << " "; } std::cout << std::endl; return false; } @@ -133,12 +132,12 @@ bool assert_container_equal(const std::map& expected, const std::map& expected, const std::map< } if(!match) { std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, expected) { + for(const typename Map::value_type& a: expected) { std::cout << "Key: " << a.first << std::endl; a.second.print(" value"); } std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, actual) { + for(const typename Map::value_type& a: actual) { std::cout << "Key: " << a.first << std::endl; a.second.print(" value"); } @@ -210,12 +209,12 @@ bool assert_container_equal(const std::vector >& expected, } if(!match) { std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename VectorPair::value_type& a, expected) { + for(const typename VectorPair::value_type& a: expected) { a.first.print( " first "); a.second.print(" second"); } std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename VectorPair::value_type& a, actual) { + for(const typename VectorPair::value_type& a: actual) { a.first.print( " first "); a.second.print(" second"); } @@ -247,9 +246,9 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e- } if(!match) { std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, expected) { a.print(" "); } + for(const typename V::value_type& a: expected) { a.print(" "); } std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, actual) { a.print(" "); } + for(const typename V::value_type& a: actual) { a.print(" "); } std::cout << std::endl; return false; } @@ -279,12 +278,12 @@ bool assert_container_equality(const std::map& expected, const std::m } if(!match) { std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, expected) { + for(const typename Map::value_type& a: expected) { std::cout << "Key: " << a.first << std::endl; std::cout << "Value: " << a.second << std::endl; } std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, actual) { + for(const typename Map::value_type& a: actual) { std::cout << "Key: " << a.first << std::endl; std::cout << "Value: " << a.second << std::endl; } @@ -316,9 +315,9 @@ bool assert_container_equality(const V& expected, const V& actual) { } if(!match) { std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, expected) { std::cout << a << " "; } + for(const typename V::value_type& a: expected) { std::cout << a << " "; } std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, actual) { std::cout << a << " "; } + for(const typename V::value_type& a: actual) { std::cout << a << " "; } std::cout << std::endl; return false; } diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 9537baa31..2d5b9d879 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -18,11 +18,12 @@ #pragma once +#include // Configuration from CMake + #include #include #include - namespace gtsam { /** @@ -37,7 +38,7 @@ namespace gtsam { * Values can operate generically on Value objects, retracting or computing * local coordinates for many Value objects of different types. * - * Inheriting from the DerivedValue class templated provides a generic implementation of + * Inheriting from the DerivedValue class template provides a generic implementation of * the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating * the need to implement these functions in your class. Note that you must inherit from * DerivedValue templated on the class you are defining. For example you cannot define diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 40d819de9..6dc9800ca 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -17,11 +17,11 @@ */ #include -#include #include #include #include #include +#include #include #include #include @@ -264,12 +264,12 @@ weightedPseudoinverse(const Vector& a, const Vector& weights) { Vector concatVectors(const std::list& vs) { size_t dim = 0; - BOOST_FOREACH(Vector v, vs) + for(Vector v: vs) dim += v.size(); Vector A(dim); size_t index = 0; - BOOST_FOREACH(Vector v, vs) { + for(Vector v: vs) { for(int d = 0; d < v.size(); d++) A(d+index) = v(d); index += v.size(); diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index cc28ac893..43644b5c4 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -138,14 +138,14 @@ struct VectorSpaceImpl { } static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + ChartJacobian H2 = boost::none) { if (H1) *H1 = Eye(v1); if (H2) *H2 = Eye(v2); return v1 + v2; } static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + ChartJacobian H2 = boost::none) { if (H1) *H1 = - Eye(v1); if (H2) *H2 = Eye(v2); return v2 - v1; @@ -311,7 +311,7 @@ struct traits > : typedef Eigen::Matrix Jacobian; typedef OptionalJacobian ChartJacobian; - static TangentVector Local(Fixed origin, Fixed other, + static TangentVector Local(const Fixed& origin, const Fixed& other, ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) (*H1) = -Jacobian::Identity(); if (H2) (*H2) = Jacobian::Identity(); @@ -320,7 +320,7 @@ struct traits > : return result; } - static Fixed Retract(Fixed origin, const TangentVector& v, + static Fixed Retract(const Fixed& origin, const TangentVector& v, ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) (*H1) = Jacobian::Identity(); if (H2) (*H2) = Jacobian::Identity(); diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index f6e2848f6..7bc5949cc 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -13,10 +13,10 @@ * @file cholesky.cpp * @brief Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky * @author Richard Roberts + * @author Frank Dellaert * @date Nov 5, 2010 */ -#include #include #include @@ -27,64 +27,53 @@ using namespace std; namespace gtsam { - static const double negativePivotThreshold = -1e-1; - static const double zeroPivotThreshold = 1e-6; - static const double underconstrainedPrior = 1e-5; - static const int underconstrainedExponentDifference = 12; +static const double negativePivotThreshold = -1e-1; +static const double zeroPivotThreshold = 1e-6; +static const double underconstrainedPrior = 1e-5; +static const int underconstrainedExponentDifference = 12; /* ************************************************************************* */ static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) { - - const bool debug = ISDEBUG("choleskyCareful"); - // Get pivot value - double alpha = ATA(k,k); + double alpha = ATA(k, k); // Correct negative pivots from round-off error - if(alpha < negativePivotThreshold) { - if(debug) { - cout << "pivot = " << alpha << endl; - print(ATA, "Partially-factorized matrix: "); - } + if (alpha < negativePivotThreshold) { return -1; - } else if(alpha < 0.0) + } else if (alpha < 0.0) alpha = 0.0; - + const double beta = sqrt(alpha); - if(beta > zeroPivotThreshold) { + if (beta > zeroPivotThreshold) { const double betainv = 1.0 / beta; // Update k,k - ATA(k,k) = beta; + ATA(k, k) = beta; - if(k < (order-1)) { + if (k < (order - 1)) { // Update A(k,k+1:end) <- A(k,k+1:end) / beta typedef Matrix::RowXpr::SegmentReturnType BlockRow; - BlockRow V = ATA.row(k).segment(k+1, order-(k+1)); + BlockRow V = ATA.row(k).segment(k + 1, order - (k + 1)); V *= betainv; // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha - ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V; -// ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView() -// .rankUpdate(V.adjoint(), -1); + ATA.block(k + 1, k + 1, order - (k + 1), order - (k + 1)) -= V.transpose() * V; + // ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView() + // .rankUpdate(V.adjoint(), -1); } return 1; } else { // For zero pivots, add the underconstrained variable prior - ATA(k,k) = underconstrainedPrior; - for(size_t j=k+1; j choleskyCareful(Matrix& ATA, int order) { - - const bool debug = ISDEBUG("choleskyCareful"); - +pair choleskyCareful(Matrix& ATA, int order) { // Check that the matrix is square (we do not check for symmetry) assert(ATA.rows() == ATA.cols()); @@ -92,7 +81,7 @@ pair choleskyCareful(Matrix& ATA, int order) { const size_t n = ATA.rows(); // Negative order means factor the entire matrix - if(order < 0) + if (order < 0) order = int(n); assert(size_t(order) <= n); @@ -102,13 +91,11 @@ pair choleskyCareful(Matrix& ATA, int order) { bool success = true; // Factor row-by-row - for(size_t k = 0; k < size_t(order); ++k) { + for (size_t k = 0; k < size_t(order); ++k) { int stepResult = choleskyStep(ATA, k, size_t(order)); - if(stepResult == 1) { - if(debug) cout << "choleskyCareful: Factored through " << k << endl; - if(debug) print(ATA, "ATA: "); - maxrank = k+1; - } else if(stepResult == -1) { + if (stepResult == 1) { + maxrank = k + 1; + } else if (stepResult == -1) { success = false; break; } /* else if(stepResult == 0) Found zero pivot */ @@ -118,72 +105,54 @@ pair choleskyCareful(Matrix& ATA, int order) { } /* ************************************************************************* */ -bool choleskyPartial(Matrix& ABC, size_t nFrontal) { - +bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) { gttic(choleskyPartial); + if (nFrontal == 0) + return true; - const bool debug = ISDEBUG("choleskyPartial"); + assert(ABC.cols() == ABC.rows()); + const Eigen::DenseIndex n = ABC.rows() - topleft; + assert(n >= 0 && nFrontal <= size_t(n)); - assert(ABC.rows() == ABC.cols()); - assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows())); + // Create views on blocks + auto A = ABC.block(topleft, topleft, nFrontal, nFrontal); + auto B = ABC.block(topleft, topleft + nFrontal, nFrontal, n - nFrontal); + auto C = ABC.block(topleft + nFrontal, topleft + nFrontal, n - nFrontal, n - nFrontal); - const size_t n = ABC.rows(); - - // Compute Cholesky factorization of A, overwrites A. - gttic(lld); - Eigen::ComputationInfo lltResult; - if(nFrontal > 0) - { - Eigen::LLT llt = ABC.block(0, 0, nFrontal, nFrontal).selfadjointView().llt(); - ABC.block(0, 0, nFrontal, nFrontal).triangularView() = llt.matrixU(); - lltResult = llt.info(); - } - else - { - lltResult = Eigen::Success; - } - gttoc(lld); - - if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView()) << endl; + // Compute Cholesky factorization A = R'*R, overwrites A. + gttic(LLT); + Eigen::LLT llt(A); + Eigen::ComputationInfo lltResult = llt.info(); + if (lltResult != Eigen::Success) + return false; + auto R = A.triangularView(); + R = llt.matrixU(); + gttoc(LLT); // Compute S = inv(R') * B gttic(compute_S); - if(n - nFrontal > 0) { - ABC.topLeftCorner(nFrontal,nFrontal).triangularView().transpose().solveInPlace( - ABC.topRightCorner(nFrontal, n-nFrontal)); - } - if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; + if (nFrontal < n) + R.transpose().solveInPlace(B); gttoc(compute_S); // Compute L = C - S' * S gttic(compute_L); - if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; - if(n - nFrontal > 0) - ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().rankUpdate( - ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); - if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; + if (nFrontal < n) + C.selfadjointView().rankUpdate(B.transpose(), -1.0); gttoc(compute_L); // Check last diagonal element - Eigen does not check it - bool ok; - if(lltResult == Eigen::Success) { - if(nFrontal >= 2) { - int exp2, exp1; - (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2); - (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1); - ok = (exp2 - exp1 < underconstrainedExponentDifference); - } else if(nFrontal == 1) { - int exp1; - (void)frexp(ABC(0,0), &exp1); - ok = (exp1 > -underconstrainedExponentDifference); - } else { - ok = true; - } + if (nFrontal >= 2) { + int exp2, exp1; + (void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2); + (void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1); + return (exp2 - exp1 < underconstrainedExponentDifference); + } else if (nFrontal == 1) { + int exp1; + (void)frexp(R(0, 0), &exp1); + return (exp1 > -underconstrainedExponentDifference); } else { - ok = false; + return true; } - - return ok; -} - } +} // namespace gtsam diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 3377ab251..5e3276ff0 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -55,10 +55,12 @@ GTSAM_EXPORT std::pair choleskyCareful(Matrix& ATA, int order = -1) * nFrontal determines the split between A, B, and C, with A being of size * nFrontal x nFrontal. * + * if non-zero, factorization proceeds in bottom-right corner starting at topleft + * * @return \c true if the decomposition is successful, \c false if \c A was * not positive-definite. */ -GTSAM_EXPORT bool choleskyPartial(Matrix& ABC, size_t nFrontal); +GTSAM_EXPORT bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft=0); } diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h index f9c424e95..68632addc 100644 --- a/gtsam/base/deprecated/LieScalar.h +++ b/gtsam/base/deprecated/LieScalar.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 6cd28b951..dcb0425b7 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -320,8 +320,8 @@ typename internal::FixedSizeMatrix::type numericalDerivative41( /** * Compute numerical derivative in argument 2 of 4-argument function * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value + * @param x1 first argument value + * @param x2 n-dimensional second argument value * @param x3 third argument value * @param x4 fourth argument value * @param delta increment for numerical derivative @@ -333,11 +333,53 @@ typename internal::FixedSizeMatrix::type numericalDerivative42( const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), - "Template argument X1 must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X2 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, _1, x3, x4), x2, delta); } +/** + * Compute numerical derivative in argument 3 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 first argument value + * @param x2 second argument value + * @param x3 n-dimensional third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative43( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X3 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, _1, x4), x3, delta); +} + +/** + * Compute numerical derivative in argument 4 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 n-dimensional fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative44( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X4 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, x3, _1), x4, delta); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 5120e9ac6..9b80658cf 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -23,10 +23,23 @@ using namespace gtsam; using namespace std; /* ************************************************************************* */ -TEST(cholesky, choleskyPartial) { +TEST(cholesky, choleskyPartial0) { // choleskyPartial should only use the upper triangle, so this represents a // symmetric matrix. + Matrix ABC(3,3); + ABC << 4.0375, 3.4584, 3.5735, + 0., 4.7267, 3.8423, + 0., 0., 5.1600; + + // Test passing 0 frontals to partialCholesky + Matrix RSL(ABC); + choleskyPartial(RSL, 0); + EXPECT(assert_equal(ABC, RSL, 1e-9)); +} + +/* ************************************************************************* */ +TEST(cholesky, choleskyPartial) { Matrix ABC = (Matrix(7,7) << 4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063, 0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914, diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index d900e1779..cf32f1c3b 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -20,7 +20,6 @@ #include -#include #include #include #include @@ -70,7 +69,7 @@ TEST(DSFBase, mergePairwiseMatches) { // Merge matches DSFBase dsf(7); // We allow for keys 0..6 - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.merge(m.first,m.second); // Each point is now associated with a set, represented by one of its members @@ -206,7 +205,7 @@ TEST(DSFVector, mergePairwiseMatches) { // Merge matches DSFVector dsf(keys); - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.merge(m.first,m.second); // Check that we have two connected components, 1,2,3 and 4,5,6 diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index bce9a6036..f405bdaf1 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -19,7 +19,6 @@ #include #include #include -#include namespace gtsam { diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 093f22961..88a6fa825 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -907,10 +906,6 @@ TEST(Matrix, weighted_elimination ) Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2).finished(); Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished(); - Vector r; - double di, sigma; - size_t i; - // perform elimination Matrix A1 = A; Vector b1 = b; @@ -918,8 +913,11 @@ TEST(Matrix, weighted_elimination ) weighted_eliminate(A1, b1, sigmas); // unpack and verify - i = 0; - BOOST_FOREACH(boost::tie(r, di, sigma), solution){ + size_t i = 0; + for (const auto& tuple : solution) { + Vector r; + double di, sigma; + boost::tie(r, di, sigma) = tuple; EXPECT(assert_equal(r, expectedR.row(i))); // verify r DOUBLES_EQUAL(d(i), di, 1e-8); // verify d DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp index da193aec5..c24e12c25 100644 --- a/gtsam/base/tests/testSymmetricBlockMatrix.cpp +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -40,58 +40,41 @@ TEST(SymmetricBlockMatrix, ReadBlocks) Matrix expected1 = (Matrix(2, 2) << 22, 23, 23, 29).finished(); - Matrix actual1 = testBlockMatrix(1, 1); - // Test only writing the upper triangle for efficiency - Matrix actual1t = Z_2x2; - actual1t.triangularView() = testBlockMatrix(1, 1).triangularView(); + Matrix actual1 = testBlockMatrix.diagonalBlock(1); EXPECT(assert_equal(expected1, actual1)); - EXPECT(assert_equal(Matrix(expected1.triangularView()), actual1t)); // Above the diagonal Matrix expected2 = (Matrix(3, 2) << 4, 5, 10, 11, 16, 17).finished(); - Matrix actual2 = testBlockMatrix(0, 1); + Matrix actual2 = testBlockMatrix.aboveDiagonalBlock(0, 1); EXPECT(assert_equal(expected2, actual2)); - - // Below the diagonal - Matrix expected3 = (Matrix(2, 3) << - 4, 10, 16, - 5, 11, 17).finished(); - Matrix actual3 = testBlockMatrix(1, 0); - EXPECT(assert_equal(expected3, actual3)); } /* ************************************************************************* */ TEST(SymmetricBlockMatrix, WriteBlocks) { // On the diagonal - Matrix expected1 = testBlockMatrix(1, 1); + Matrix expected1 = testBlockMatrix.diagonalBlock(1); SymmetricBlockMatrix bm1 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); - bm1(1, 1) = expected1.selfadjointView(); // Verified with debugger that this only writes the upper triangle - Matrix actual1 = bm1(1, 1); + + bm1.setDiagonalBlock(1, expected1); + Matrix actual1 = bm1.diagonalBlock(1); EXPECT(assert_equal(expected1, actual1)); - // On the diagonal - Matrix expected1p = testBlockMatrix(1, 1); - SymmetricBlockMatrix bm1p = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); - bm1p(1, 1) = expected1p; // Verified with debugger that this only writes the upper triangle - Matrix actual1p = bm1p(1, 1); - EXPECT(assert_equal(expected1p, actual1p)); - // Above the diagonal - Matrix expected2 = testBlockMatrix(0, 1); + Matrix expected2 = testBlockMatrix.aboveDiagonalBlock(0, 1); SymmetricBlockMatrix bm2 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); - bm2(0, 1) = expected2; - Matrix actual2 = bm2(0, 1); + bm2.setOffDiagonalBlock(0, 1, expected2); + Matrix actual2 = bm2.aboveDiagonalBlock(0, 1); EXPECT(assert_equal(expected2, actual2)); // Below the diagonal - Matrix expected3 = testBlockMatrix(1, 0); + Matrix expected3 = testBlockMatrix.aboveDiagonalBlock(0, 1).transpose(); SymmetricBlockMatrix bm3 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); - bm3(1, 0) = expected3; - Matrix actual3 = bm3(1, 0); + bm3.setOffDiagonalBlock(1, 0, expected3); + Matrix actual3 = bm3.aboveDiagonalBlock(0, 1).transpose(); EXPECT(assert_equal(expected3, actual3)); } @@ -103,30 +86,16 @@ TEST(SymmetricBlockMatrix, Ranges) 22, 23, 24, 23, 29, 30, 24, 30, 36).finished(); - Matrix actual1 = testBlockMatrix.range(1, 3, 1, 3).selfadjointView(); - Matrix actual1a = testBlockMatrix.range(1, 3, 1, 3); + Matrix actual1 = testBlockMatrix.selfadjointView(1, 3); EXPECT(assert_equal(expected1, actual1)); - EXPECT(assert_equal(expected1, actual1a)); // Above the diagonal - Matrix expected2 = (Matrix(3, 1) << - 24, - 30, - 36).finished(); - Matrix actual2 = testBlockMatrix.range(1, 3, 2, 3).knownOffDiagonal(); - Matrix actual2a = testBlockMatrix.range(1, 3, 2, 3); + Matrix expected2 = (Matrix(3, 3) << + 4, 5, 6, + 10, 11, 12, + 16, 17, 18).finished(); + Matrix actual2 = testBlockMatrix.aboveDiagonalRange(0, 1, 1, 3); EXPECT(assert_equal(expected2, actual2)); - EXPECT(assert_equal(expected2, actual2a)); - - // Below the diagonal - Matrix expected3 = (Matrix(3, 3) << - 4, 10, 16, - 5, 11, 17, - 6, 12, 18).finished(); - Matrix actual3 = testBlockMatrix.range(1, 3, 0, 1).knownOffDiagonal(); - Matrix actual3a = testBlockMatrix.range(1, 3, 0, 1); - EXPECT(assert_equal(expected3, actual3)); - EXPECT(assert_equal(expected3, actual3a)); } /* ************************************************************************* */ @@ -152,34 +121,51 @@ TEST(SymmetricBlockMatrix, expressions) Matrix b = (Matrix(1, 2) << 5, 6).finished(); SymmetricBlockMatrix bm1(list_of(2)(3)(1)); - bm1.full().triangularView().setZero(); - bm1(1, 1).selfadjointView().rankUpdate(a.transpose()); - EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm1.full().selfadjointView())); + bm1.setZero(); + bm1.diagonalBlock(1).rankUpdate(a.transpose()); + EXPECT(assert_equal(Matrix(expected1.selfadjointView()), bm1.selfadjointView())); SymmetricBlockMatrix bm2(list_of(2)(3)(1)); - bm2.full().triangularView().setZero(); - bm2(0, 1).knownOffDiagonal() += b.transpose() * a; - EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm2.full().selfadjointView())); + bm2.setZero(); + bm2.updateOffDiagonalBlock(0, 1, b.transpose() * a); + EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm2.selfadjointView())); SymmetricBlockMatrix bm3(list_of(2)(3)(1)); - bm3.full().triangularView().setZero(); - bm3(1, 0).knownOffDiagonal() += a.transpose() * b; - EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm3.full().selfadjointView())); + bm3.setZero(); + bm3.updateOffDiagonalBlock(1, 0, a.transpose() * b); + EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm3.selfadjointView())); SymmetricBlockMatrix bm4(list_of(2)(3)(1)); - bm4.full().triangularView().setZero(); - bm4(1, 1) += expected1(1, 1); - EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm4.full().selfadjointView())); + bm4.setZero(); + bm4.updateDiagonalBlock(1, expected1.diagonalBlock(1)); + EXPECT(assert_equal(Matrix(expected1.selfadjointView()), bm4.selfadjointView())); SymmetricBlockMatrix bm5(list_of(2)(3)(1)); - bm5.full().triangularView().setZero(); - bm5(0, 1) += expected2(0, 1); - EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm5.full().selfadjointView())); + bm5.setZero(); + bm5.updateOffDiagonalBlock(0, 1, expected2.aboveDiagonalBlock(0, 1)); + EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm5.selfadjointView())); SymmetricBlockMatrix bm6(list_of(2)(3)(1)); - bm6.full().triangularView().setZero(); - bm6(1, 0) += expected2(1, 0); - EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm6.full().selfadjointView())); + bm6.setZero(); + bm6.updateOffDiagonalBlock(1, 0, expected2.aboveDiagonalBlock(0, 1).transpose()); + EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm6.selfadjointView())); +} + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, inverseInPlace) { + // generate an invertible matrix + const Vector3 a(1.0, 0.2, 2.0), b(0.3, 0.8, -1.0), c(0.1, 0.2, 0.7); + Matrix inputMatrix(3, 3); + inputMatrix.setZero(); + inputMatrix += a * a.transpose(); + inputMatrix += b * b.transpose(); + inputMatrix += c * c.transpose(); + const Matrix expectedInverse = inputMatrix.inverse(); + + SymmetricBlockMatrix symmMatrix(list_of(2)(1), inputMatrix); + // invert in place + symmMatrix.invertInPlace(); + EXPECT(assert_equal(expectedInverse, symmMatrix.selfadjointView())); } /* ************************************************************************* */ diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index b76746ba0..ac58fcca7 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -20,7 +20,6 @@ #include #include -#include #include #include @@ -66,7 +65,7 @@ TimingOutline::TimingOutline(const std::string& label, size_t id) : size_t TimingOutline::time() const { size_t time = 0; bool hasChildren = false; - BOOST_FOREACH(const ChildMap::value_type& child, children_) { + for(const ChildMap::value_type& child: children_) { time += child.second->time(); hasChildren = true; } @@ -86,11 +85,11 @@ void TimingOutline::print(const std::string& outline) const { // Order children typedef FastMap > ChildOrder; ChildOrder childOrder; - BOOST_FOREACH(const ChildMap::value_type& child, children_) { + for(const ChildMap::value_type& child: children_) { childOrder[child.second->myOrder_] = child.second; } // Print children - BOOST_FOREACH(const ChildOrder::value_type order_child, childOrder) { + for(const ChildOrder::value_type order_child: childOrder) { std::string childOutline(outline); childOutline += "| "; order_child.second->print(childOutline); @@ -130,7 +129,7 @@ void TimingOutline::print2(const std::string& outline, std::cout << std::endl; } - BOOST_FOREACH(const ChildMap::value_type& child, children_) { + for(const ChildMap::value_type& child: children_) { std::string childOutline(outline); if (n_ == 0) { child.second->print2(childOutline, childTotal); @@ -210,7 +209,7 @@ void TimingOutline::finishedIteration() { if (tMin_ == 0 || tIt_ < tMin_) tMin_ = tIt_; tIt_ = 0; - BOOST_FOREACH(ChildMap::value_type& child, children_) { + for(ChildMap::value_type& child: children_) { child.second->finishedIteration(); } } diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 12a29e45b..7a88f72eb 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -29,8 +29,6 @@ #include #include #include -#include -#include namespace gtsam { @@ -91,7 +89,7 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, // Add roots to stack (insert such that they are visited and processed in order { typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& root, forest.roots()) + for(const sharedNode& root: forest.roots()) stack.insert(insertLocation, TraversalNode(root, rootData)); } @@ -112,7 +110,7 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData)); typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& child, node.treeNode->children) + for(const sharedNode& child: node.treeNode->children) stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); node.expanded = true; } @@ -159,7 +157,6 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, #ifdef GTSAM_USE_TBB // Typedefs typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; tbb::task::spawn_root_and_wait( internal::CreateRootTask(forest.roots(), rootData, visitorPre, diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index c1df47a01..9b2dae3d0 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -20,7 +20,6 @@ #include #include -#include #ifdef GTSAM_USE_TBB # include @@ -45,8 +44,9 @@ namespace gtsam { boost::shared_ptr myData; VISITOR_POST& visitorPost; - PostOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_POST& visitorPost) : - treeNode(treeNode), myData(myData), visitorPost(visitorPost) {} + PostOrderTask(const boost::shared_ptr& treeNode, + const boost::shared_ptr& myData, VISITOR_POST& visitorPost) + : treeNode(treeNode), myData(myData), visitorPost(visitorPost) {} tbb::task* execute() { @@ -72,10 +72,15 @@ namespace gtsam { bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, - VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) : - treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} + VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, + bool makeNewTasks = true) + : treeNode(treeNode), + myData(myData), + visitorPre(visitorPre), + visitorPost(visitorPost), + problemSizeThreshold(problemSizeThreshold), + makeNewTasks(makeNewTasks), + isPostOrderPhase(false) {} tbb::task* execute() { @@ -94,33 +99,28 @@ namespace gtsam { // Allocate post-order task as a continuation isPostOrderPhase = true; recycle_as_continuation(); - //PostOrderTask& postOrderTask = - // *new(allocate_continuation()) PostOrderTask(treeNode, myData, visitorPost); bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); tbb::task* firstChild = 0; tbb::task_list childTasks; - BOOST_FOREACH(const boost::shared_ptr& child, treeNode->children) + for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling // allocate_child so that if visitorPre throws an exception, we will not have // allocated an extra child, this causes a TBB error. - boost::shared_ptr childData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(child, *myData)); - //childTasks.push_back(*new(postOrderTask.allocate_child()) - // PreOrderTask(child, childData, visitorPre, visitorPost, - // problemSizeThreshold, overThreshold)); - tbb::task* childTask = new(allocate_child()) - PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if(firstChild) + boost::shared_ptr childData = boost::allocate_shared( + tbb::scalable_allocator(), visitorPre(child, *myData)); + tbb::task* childTask = + new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, overThreshold); + if (firstChild) childTasks.push_back(*childTask); else firstChild = childTask; } // If we have child tasks, start subtasks and wait for them to complete - //postOrderTask.set_ref_count((int) treeNode->children.size()); set_ref_count((int)treeNode->children.size()); spawn(childTasks); return firstChild; @@ -143,7 +143,7 @@ namespace gtsam { void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) { - BOOST_FOREACH(const boost::shared_ptr& child, node->children) + for(const boost::shared_ptr& child: node->children) { DATA childData = visitorPre(child, myData); processNodeRecursively(child, childData); @@ -174,7 +174,7 @@ namespace gtsam { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children tbb::task_list tasks; - BOOST_FOREACH(const boost::shared_ptr& root, roots) + for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); tasks.push_back(*new(allocate_child()) diff --git a/gtsam/base/treeTraversal/statistics.h b/gtsam/base/treeTraversal/statistics.h index 805c69758..7a5c7d256 100644 --- a/gtsam/base/treeTraversal/statistics.h +++ b/gtsam/base/treeTraversal/statistics.h @@ -63,7 +63,7 @@ namespace gtsam { { int largestProblemSize = 0; int secondLargestProblemSize = 0; - BOOST_FOREACH(const boost::shared_ptr& child, node->children) + for(const boost::shared_ptr& child: node->children) { if (child->problemSize() > largestProblemSize) { diff --git a/gtsam/config.h.in b/gtsam/config.h.in index f9a576d14..8433f19b0 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -64,7 +64,10 @@ #cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4 // Publish flag about Eigen typedef -#cmakedefine GTSAM_USE_VECTOR3_POINTS +#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION + +// Support Metis-based nested dissection +#cmakedefine GTSAM_TANGENT_PREINTEGRATION diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h index 709c7350e..3665d6dfa 100644 --- a/gtsam/discrete/Assignment.h +++ b/gtsam/discrete/Assignment.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include #include @@ -36,7 +35,7 @@ namespace gtsam { public: void print(const std::string& s = "Assignment: ") const { std::cout << s << ": "; - BOOST_FOREACH(const typename Assignment::value_type& keyValue, *this) + for(const typename Assignment::value_type& keyValue: *this) std::cout << "(" << keyValue.first << ", " << keyValue.second << ")"; std::cout << std::endl; } @@ -65,7 +64,7 @@ namespace gtsam { std::vector > allPossValues; Assignment values; typedef std::pair DiscreteKey; - BOOST_FOREACH(const DiscreteKey& key, keys) + for(const DiscreteKey& key: keys) values[key.first] = 0; //Initialize from 0 while (1) { allPossValues.push_back(values); diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index c1648888e..ecc8533c1 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -24,7 +24,6 @@ #include #include -#include #include #include using boost::assign::operator+=; @@ -310,7 +309,7 @@ namespace gtsam { label_(label), allSame_(true) { branches_.reserve(f.branches_.size()); // reserve space - BOOST_FOREACH (const NodePtr& branch, f.branches_) + for (const NodePtr& branch: f.branches_) push_back(branch->apply(op)); } @@ -332,7 +331,7 @@ namespace gtsam { // If second argument of binary op is Leaf node, recurse on branches NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { boost::shared_ptr h(new Choice(label(), nrChoices())); - BOOST_FOREACH(NodePtr branch, branches_) + for(NodePtr branch: branches_) h->push_back(fL.apply_f_op_g(*branch, op)); return Unique(h); } @@ -347,7 +346,7 @@ namespace gtsam { template NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const { boost::shared_ptr h(new Choice(label(), nrChoices())); - BOOST_FOREACH(const NodePtr& branch, branches_) + for(const NodePtr& branch: branches_) h->push_back(branch->apply_f_op_g(gL, op)); return Unique(h); } @@ -359,7 +358,7 @@ namespace gtsam { // second case, not label of interest, just recurse boost::shared_ptr r(new Choice(label_, branches_.size())); - BOOST_FOREACH(const NodePtr& branch, branches_) + for(const NodePtr& branch: branches_) r->push_back(branch->choose(label, index)); return Unique(r); } @@ -593,7 +592,7 @@ namespace gtsam { // put together via Shannon expansion otherwise not sorted. std::vector functions; - BOOST_FOREACH(const MXNodePtr& branch, choice->branches()) { + for(const MXNodePtr& branch: choice->branches()) { LY converted(convert(branch, map, op)); functions += converted; } diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 9ed88bc3d..f83349436 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -21,7 +21,6 @@ #include #include -#include #include using namespace std; @@ -66,11 +65,11 @@ namespace gtsam { ADT::Binary op) const { map cs; // new cardinalities // make unique key-cardinality map - BOOST_FOREACH(Key j, keys()) cs[j] = cardinality(j); - BOOST_FOREACH(Key j, f.keys()) cs[j] = f.cardinality(j); + for(Key j: keys()) cs[j] = cardinality(j); + for(Key j: f.keys()) cs[j] = f.cardinality(j); // Convert map into keys DiscreteKeys keys; - BOOST_FOREACH(const DiscreteKey& key, cs) + for(const DiscreteKey& key: cs) keys.push_back(key); // apply operand ADT result = ADT::apply(f, op); diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index f7253ec24..54cce56be 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -22,7 +22,6 @@ #include #include -#include #include #include diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 3b3939674..84a80c565 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -19,7 +19,9 @@ #include #include #include + #include +#include namespace gtsam { @@ -46,7 +48,7 @@ namespace gtsam { double DiscreteBayesNet::evaluate(const DiscreteConditional::Values & values) const { // evaluate all conditionals and multiply double result = 1.0; - BOOST_FOREACH(DiscreteConditional::shared_ptr conditional, *this) + for(DiscreteConditional::shared_ptr conditional: *this) result *= (*conditional)(values); return result; } @@ -55,7 +57,7 @@ namespace gtsam { DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const { // solve each node in turn in topological sort order (parents first) DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, *this) + for (auto conditional: boost::adaptors::reverse(*this)) conditional->solveInPlace(*result); return result; } @@ -64,7 +66,7 @@ namespace gtsam { DiscreteFactor::sharedValues DiscreteBayesNet::sample() const { // sample each node in turn in topological sort order (parents first) DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, *this) + for (auto conditional: boost::adaptors::reverse(*this)) conditional->sampleInPlace(*result); return result; } diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index f21a705ff..4a918ef02 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -87,7 +87,7 @@ bool DiscreteConditional::equals(const DiscreteFactor& other, Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const { ADT pFS(*this); Key j; size_t value; - BOOST_FOREACH(Key key, parents()) + for(Key key: parents()) try { j = (key); value = parentsValues.at(j); @@ -111,7 +111,7 @@ void DiscreteConditional::solveInPlace(Values& values) const { double maxP = 0; DiscreteKeys keys; - BOOST_FOREACH(Key idx, frontals()) { + for(Key idx: frontals()) { DiscreteKey dk(idx, cardinality(idx)); keys & dk; } @@ -119,7 +119,7 @@ void DiscreteConditional::solveInPlace(Values& values) const { vector allPosbValues = cartesianProduct(keys); // Find the MPE - BOOST_FOREACH(Values& frontalVals, allPosbValues) { + for(Values& frontalVals: allPosbValues) { double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) // Update MPE solution if better if (pValueS > maxP) { @@ -129,7 +129,7 @@ void DiscreteConditional::solveInPlace(Values& values) const { } //set values (inPlace) to mpe - BOOST_FOREACH(Key j, frontals()) { + for(Key j: frontals()) { values[j] = mpe[j]; } } diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index e9f57c30d..c101653d2 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -18,7 +18,6 @@ */ #include -#include using namespace std; diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index c2128c776..af11d4b14 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -41,7 +41,7 @@ namespace gtsam { /* ************************************************************************* */ KeySet DiscreteFactorGraph::keys() const { KeySet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) + for(const sharedFactor& factor: *this) if (factor) keys.insert(factor->begin(), factor->end()); return keys; } @@ -49,7 +49,7 @@ namespace gtsam { /* ************************************************************************* */ DecisionTreeFactor DiscreteFactorGraph::product() const { DecisionTreeFactor result; - BOOST_FOREACH(const sharedFactor& factor, *this) + for(const sharedFactor& factor: *this) if (factor) result = (*factor) * result; return result; } @@ -58,7 +58,7 @@ namespace gtsam { double DiscreteFactorGraph::operator()( const DiscreteFactor::Values &values) const { double product = 1.0; - BOOST_FOREACH( const sharedFactor& factor, factors_ ) + for( const sharedFactor& factor: factors_ ) product *= (*factor)(values); return product; } @@ -78,7 +78,7 @@ namespace gtsam { // /* ************************************************************************* */ // void DiscreteFactorGraph::permuteWithInverse( // const Permutation& inversePermutation) { -// BOOST_FOREACH(const sharedFactor& factor, factors_) { +// for(const sharedFactor& factor: factors_) { // if(factor) // factor->permuteWithInverse(inversePermutation); // } @@ -87,7 +87,7 @@ namespace gtsam { // /* ************************************************************************* */ // void DiscreteFactorGraph::reduceWithInverse( // const internal::Reduction& inverseReduction) { -// BOOST_FOREACH(const sharedFactor& factor, factors_) { +// for(const sharedFactor& factor: factors_) { // if(factor) // factor->reduceWithInverse(inverseReduction); // } @@ -107,7 +107,7 @@ namespace gtsam { // PRODUCT: multiply all factors gttic(product); DecisionTreeFactor product; - BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors) + for(const DiscreteFactor::shared_ptr& factor: factors) product = (*factor) * product; gttoc(product); diff --git a/gtsam/discrete/DiscreteJunctionTree.cpp b/gtsam/discrete/DiscreteJunctionTree.cpp index 8e6d0f4d8..0e6e2b73e 100644 --- a/gtsam/discrete/DiscreteJunctionTree.cpp +++ b/gtsam/discrete/DiscreteJunctionTree.cpp @@ -23,7 +23,7 @@ namespace gtsam { // Instantiate base classes - template class ClusterTree; + template class EliminatableClusterTree; template class JunctionTree; /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteJunctionTree.h b/gtsam/discrete/DiscreteJunctionTree.h index 23924acfd..8c00065d9 100644 --- a/gtsam/discrete/DiscreteJunctionTree.h +++ b/gtsam/discrete/DiscreteJunctionTree.h @@ -26,8 +26,8 @@ namespace gtsam { class DiscreteEliminationTree; /** - * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with - * the additional property that it represents the clique tree associated with a Bayes net. + * An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, + * with the additional property that it represents the clique tree associated with a Bayes net. * * In GTSAM a junction tree is an intermediate data structure in multifrontal * variable elimination. Each node is a cluster of factors, along with a @@ -39,7 +39,7 @@ namespace gtsam { * BayesTree stores conditionals, that are the product of eliminating the factors in the * corresponding JunctionTree cliques. * - * The tree structure and elimination method are exactly analagous to the EliminationTree, + * The tree structure and elimination method are exactly analogous to the EliminationTree, * except that in the JunctionTree, at each node multiple variables are eliminated at a time. * * \addtogroup Multifrontal @@ -53,7 +53,7 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** - * Build the elimination tree of a factor graph using pre-computed column structure. + * Build the elimination tree of a factor graph using precomputed column structure. * @param factorGraph The factor graph for which to build the elimination tree * @param structure The set of factors involving each variable. If this is not * precomputed, you can call the Create(const FactorGraph&) diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp index 828a93eec..b4fd2e5a1 100644 --- a/gtsam/discrete/DiscreteKey.cpp +++ b/gtsam/discrete/DiscreteKey.cpp @@ -18,7 +18,6 @@ #include #include // for key names -#include // FOREACH #include "DiscreteKey.h" namespace gtsam { @@ -34,7 +33,7 @@ namespace gtsam { vector DiscreteKeys::indices() const { vector < Key > js; - BOOST_FOREACH(const DiscreteKey& key, *this) + for(const DiscreteKey& key: *this) js.push_back(key.first); return js; } @@ -42,7 +41,7 @@ namespace gtsam { map DiscreteKeys::cardinalities() const { map cs; cs.insert(begin(),end()); -// BOOST_FOREACH(const DiscreteKey& key, *this) +// for(const DiscreteKey& key: *this) // cs.insert(key); return cs; } diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index 11dddee66..c4cdbe0ef 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -54,7 +54,7 @@ namespace gtsam { void Potentials::print(const string& s, const KeyFormatter& formatter) const { cout << s << "\n Cardinalities: "; - BOOST_FOREACH(const DiscreteKey& key, cardinalities_) + for(const DiscreteKey& key: cardinalities_) cout << formatter(key.first) << "=" << formatter(key.second) << " "; cout << endl; ADT::print(" "); @@ -68,11 +68,11 @@ namespace gtsam { // map ordering; // // // Get the original keys from cardinalities_ -// BOOST_FOREACH(const DiscreteKey& key, cardinalities_) +// for(const DiscreteKey& key: cardinalities_) // keys & key; // // // Perform Permutation -// BOOST_FOREACH(DiscreteKey& key, keys) { +// for(DiscreteKey& key: keys) { // ordering[key.first] = remapping[key.first]; // key.first = ordering[key.first]; // } diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index 9e63b957d..0ee5c63b8 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -17,7 +17,6 @@ */ #include -#include #include "Signature.h" @@ -125,7 +124,7 @@ namespace gtsam { DiscreteKeys Signature::discreteKeysParentsFirst() const { DiscreteKeys keys; - BOOST_FOREACH(const DiscreteKey& key, parents_) + for(const DiscreteKey& key: parents_) keys.push_back(key); keys.push_back(key_); return keys; @@ -134,7 +133,7 @@ namespace gtsam { vector Signature::indices() const { vector js; js.push_back(key_.first); - BOOST_FOREACH(const DiscreteKey& key, parents_) + for(const DiscreteKey& key: parents_) js.push_back(key.first); return js; } @@ -142,8 +141,8 @@ namespace gtsam { vector Signature::cpt() const { vector cpt; if (table_) { - BOOST_FOREACH(const Row& row, *table_) - BOOST_FOREACH(const double& x, row) + for(const Row& row: *table_) + for(const double& x: row) cpt.push_back(x); } return cpt; @@ -171,7 +170,7 @@ namespace gtsam { // qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); // using full grammar parser::parse_table(spec, table); if (success) { - BOOST_FOREACH(Row& row, table) + for(Row& row: table) normalize(row); table_.reset(table); } @@ -180,7 +179,7 @@ namespace gtsam { Signature& Signature::operator=(const Table& t) { Table table = t; - BOOST_FOREACH(Row& row, table) + for(Row& row: table) normalize(row); table_.reset(table); return *this; diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index d6902bbef..9c3f4bd63 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -25,7 +25,6 @@ #define DISABLE_TIMING #include -#include #include #include #include @@ -66,7 +65,7 @@ void dot(const T&f, const string& filename) { typename DecisionTree::Node::Ptr DecisionTree::Choice::apply_fC_op_gL( Cache& cache, const Leaf& gL, Mul op) const { Ptr h(new Choice(label(), cardinality())); - BOOST_FOREACH(const NodePtr& branch, branches_) + for(const NodePtr& branch: branches_) h->push_back(branch->apply_f_op_g(cache, gL, op)); return Unique(cache, h); } @@ -401,7 +400,7 @@ TEST(ADT, constructor) DiscreteKey z0(0,5), z1(1,4), z2(2,3), z3(3,2); vector table(5 * 4 * 3 * 2); double x = 0; - BOOST_FOREACH(double& t, table) + for(double& t: table) t = x++; ADT f3(z0 & z1 & z2 & z3, table); Assignment assignment; diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index c1acaf83d..93126f642 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -63,7 +63,7 @@ //// double evaluate(const DiscreteConditional::Values & values) { //// double result = (*(this->conditional_))(values); //// // evaluate all children and multiply into result -//// BOOST_FOREACH(boost::shared_ptr c, children_) +//// for(boost::shared_ptr c: children_) //// result *= c->evaluate(values); //// return result; //// } @@ -213,7 +213,7 @@ // // // calculate all shortcuts to root // DiscreteBayesTree::Nodes cliques = bayesTree.nodes(); -// BOOST_FOREACH(Clique::shared_ptr c, cliques) { +// for(Clique::shared_ptr c: cliques) { // DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete); // if (debug) { // c->printSignature(); diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 052180f7a..0fbf44097 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -321,7 +321,7 @@ struct Graph2: public std::list { /** Add a factor graph*/ // void operator +=(const Graph2& graph) { -// BOOST_FOREACH(const Factor2& f, graph) +// for(const Factor2& f: graph) // push_back(f); // } friend std::ostream& operator <<(std::ostream &os, const Graph2& graph); @@ -334,7 +334,7 @@ friend std::ostream& operator <<(std::ostream &os, const Graph2& graph); // return graph; //} std::ostream& operator <<(std::ostream &os, const Graph2& graph) { -BOOST_FOREACH(const Factor2& f, graph) +for(const Factor2& f: graph) os << f << endl; return os; } diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 27fe2f197..b1e003864 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 368ae6c98..4ad1dffa2 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -106,7 +106,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (uncalibrate(pn).distance(pi) <= tol) + if (distance2(uncalibrate(pn), pi) <= tol) break; const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; const double rr = xx + yy; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 12060c12d..2071b8792 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -144,7 +144,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (uncalibrate(pn).distance(pi) <= tol) break; + if (distance2(uncalibrate(pn), pi) <= tol) break; const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index c131d46f7..54deedfdc 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -51,6 +51,13 @@ Cal3_S2::Cal3_S2(const std::string &path) : infile.close(); } +/* ************************************************************************* */ +ostream& operator<<(ostream& os, const Cal3_S2& cal) { + os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px() + << ", py:" << cal.py() << "}"; + return os; +} + /* ************************************************************************* */ void Cal3_S2::print(const std::string& s) const { gtsam::print((Matrix)matrix(), s); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index ac4b68ccd..6ad7aeb86 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -75,6 +75,9 @@ public: /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal); + /// print with optional string void print(const std::string& s = "Cal3_S2") const; diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp new file mode 100644 index 000000000..22966ee37 --- /dev/null +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3_S2Stereo.cpp + * @brief The most common 5DOF 3D->2D calibration + Stereo baseline + * @author Chris Beall + */ + +#include + +#include + +namespace gtsam { +using namespace std; + +/* ************************************************************************* */ +void Cal3_S2Stereo::print(const std::string& s) const { + K_.print(s+"K: "); + std::cout << s << "Baseline: " << b_ << std::endl; + } + +/* ************************************************************************* */ +bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { + if (fabs(b_ - other.b_) > tol) return false; + return K_.equals(other.K_,tol); +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 365a6c40e..db49448ec 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -18,6 +18,7 @@ #pragma once #include +#include namespace gtsam { @@ -62,16 +63,10 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s = "") const { - K_.print(s+"K: "); - std::cout << s << "Baseline: " << b_ << std::endl; - } + void print(const std::string& s = "") const; /// Check if equal up to specified tolerance - bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const { - if (fabs(b_ - other.b_) > tol) return false; - return K_.equals(other.K_,tol); - } + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 2d27b4dc7..026becebe 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index b1e5917b2..f1fa509c1 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include #include @@ -28,8 +29,6 @@ namespace gtsam { -class Point2; - class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: @@ -262,6 +261,14 @@ public: /// @name Named Constructors /// @{ + // Create CalibratedCamera, with derivatives + static CalibratedCamera Create(const Pose3& pose, + OptionalJacobian H1 = boost::none) { + if (H1) + *H1 << I_6x6; + return CalibratedCamera(pose); + } + /** * Create a level camera at the given 2D pose and height * @param pose2 specifies the location and viewing direction diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 0df85d3d2..18f311a23 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -56,8 +56,11 @@ protected: // Project and fill error vector Vector b(ZDim * m); for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - Z e = predicted[i] - measured[i]; - b.segment(row) = e.vector(); + Vector bi = traits::Local(measured[i], predicted[i]); + if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan) + bi(1) = 0; + } + b.segment(row) = bi; } return b; } @@ -107,7 +110,8 @@ public: // Allocate result size_t m = this->size(); - std::vector z(m); + std::vector z; + z.reserve(m); // Allocate derivatives if (E) E->resize(ZDim * m, N); @@ -117,7 +121,7 @@ public: for (size_t i = 0; i < m; i++) { MatrixZD Fi; Eigen::Matrix Ei; - z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0); + z.emplace_back(this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0)); if (Fs) (*Fs)[i] = Fi; if (E) E->block(ZDim * i, 0) = Ei; } @@ -157,28 +161,29 @@ public: for (size_t i = 0; i < m; i++) { // for each camera const MatrixZD& Fi = Fs[i]; + const auto FiT = Fi.transpose(); const Eigen::Matrix Ei_P = // E.block(ZDim * i, 0, ZDim, N) * P; // D = (Dx2) * ZDim - augmentedHessian(i, m) = Fi.transpose() * b.segment(ZDim * i) // F' * b - - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian(i, i) = Fi.transpose() - * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi); + augmentedHessian.setDiagonalBlock(i, FiT + * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); // upper triangular part of the hessian for (size_t j = i + 1; j < m; j++) { // for each camera const MatrixZD& Fj = Fs[j]; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i, j) = -Fi.transpose() - * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj); + augmentedHessian.setOffDiagonalBlock(i, j, -FiT + * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); } } // end of for over cameras - augmentedHessian(m, m)(0, 0) += b.squaredNorm(); + augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); return augmentedHessian; } @@ -252,8 +257,6 @@ public: // G = F' * F - F' * E * P * E' * F // g = F' * (b - E * P * E' * b) - Eigen::Matrix matrixBlock; - // a single point is observed in m cameras size_t m = Fs.size(); // cameras observing current point size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group @@ -263,6 +266,7 @@ public: for (size_t i = 0; i < m; i++) { // for each camera in the current factor const MatrixZD& Fi = Fs[i]; + const auto FiT = Fi.transpose(); const Eigen::Matrix Ei_P = E.template block( ZDim * i, 0) * P; @@ -275,17 +279,15 @@ public: // information vector - store previous vector // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal() - + Fi.transpose() * b.segment(ZDim * i) // F' * b - - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + augmentedHessian.updateOffDiagonalBlock(aug_i, M, + FiT * b.segment(ZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - // main block diagonal - store previous block - matrixBlock = augmentedHessian(aug_i, aug_i); + // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // add contribution of current factor - augmentedHessian(aug_i, aug_i) = matrixBlock - + (Fi.transpose() - * (Fi - Ei_P * E.template block(ZDim * i, 0).transpose() * Fi)); + // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for now... + augmentedHessian.updateDiagonalBlock(aug_i, + ((FiT * (Fi - Ei_P * E.template block(ZDim * i, 0).transpose() * Fi))).eval()); // upper triangular part of the hessian for (size_t j = i + 1; j < m; j++) { // for each camera @@ -297,14 +299,12 @@ public: // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i, aug_j) = - augmentedHessian(aug_i, aug_j).knownOffDiagonal() - - Fi.transpose() - * (Ei_P * E.template block(ZDim * j, 0).transpose() * Fj); + augmentedHessian.updateOffDiagonalBlock(aug_i, aug_j, + -FiT * (Ei_P * E.template block(ZDim * j, 0).transpose() * Fj)); } } // end of for over cameras - augmentedHessian(M, M)(0, 0) += b.squaredNorm(); + augmentedHessian.diagonalBlock(M)(0, 0) += b.squaredNorm(); } private: diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index ac889c9d7..43ba78ea2 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -312,6 +312,16 @@ public: return Base::equals(camera, tol) && K_->equals(e->calibration(), tol); } + /// stream operator + friend std::ostream& operator<<(std::ostream &os, const PinholePose& camera) { + os << "{R: " << camera.pose().rotation().rpy().transpose(); + os << ", t: " << camera.pose().translation().transpose(); + if (!camera.K_) os << ", K: none"; + else os << ", K: " << *camera.K_; + os << "}"; + return os; + } + /// print void print(const std::string& s = "PinholePose") const { Base::print(s); diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 5101e9fc8..65047df41 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -20,7 +20,6 @@ #include #include #include -#include namespace gtsam { diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 7066e527b..74b9a2bec 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -16,13 +16,42 @@ */ #include -#include #include +#include using namespace std; namespace gtsam { +/* ************************************************************************* */ +double norm2(const Point2& p, OptionalJacobian<1,2> H) { + double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); + if (H) { + if (fabs(r) > 1e-10) + *H << p.x() / r, p.y() / r; + else + *H << 1, 1; // really infinity, why 1 ? + } + return r; +} + +/* ************************************************************************* */ +double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1, + OptionalJacobian<1, 2> H2) { + Point2 d = q - p; + if (H1 || H2) { + Matrix12 H; + double r = norm2(d, H); + if (H1) *H1 = -H; + if (H2) *H2 = H; + return r; + } else { + return d.norm(); + } +} + +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS + /* ************************************************************************* */ void Point2::print(const string& s) const { cout << s << *this << endl; @@ -30,50 +59,43 @@ void Point2::print(const string& s) const { /* ************************************************************************* */ bool Point2::equals(const Point2& q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); + return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol); } /* ************************************************************************* */ double Point2::norm(OptionalJacobian<1,2> H) const { - double r = sqrt(x_ * x_ + y_ * y_); - if (H) { - if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r; - else - *H << 1, 1; // really infinity, why 1 ? - } - return r; + return gtsam::norm2(*this, H); } /* ************************************************************************* */ double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, OptionalJacobian<1,2> H2) const { - Point2 d = point - *this; - if (H1 || H2) { - Matrix12 H; - double r = d.norm(H); - if (H1) *H1 = -H; - if (H2) *H2 = H; - return r; - } else - return d.norm(); + return gtsam::distance2(*this, point, H1, H2); } -/* - * Calculate f and h, respectively the parallel and perpendicular distance of - * the intersections of two circles along and from the line connecting the centers. - * Both are dimensionless fractions of the distance d between the circle centers. - * If the circles do not intersect or they are identical, returns boost::none. - * If one solution (touching circles, as determined by tol), h will be exactly zero. - * h is a good measure for how accurate the intersection will be, as when circles touch - * or nearly touch, the intersection is ill-defined with noisy radius measurements. - * @param R_d : R/d, ratio of radius of first circle to distance between centers - * @param r_d : r/d, ratio of radius of second circle to distance between centers - * @param tol: absolute tolerance below which we consider touching circles - */ +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point2& p) { + os << '(' << p.x() << ", " << p.y() << ')'; + return os; +} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +boost::optional CircleCircleIntersection(double R_d, double r_d, double tol) { + return circleCircleIntersection(R_d, r_d, tol); +} +std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh) { + return circleCircleIntersection(c1, c2, fh); +} +std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) { + return circleCircleIntersection(c1, r1, c2, r2, tol); +} +#endif + +#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS + /* ************************************************************************* */ // Math inspired by http://paulbourke.net/geometry/circlesphere/ -boost::optional Point2::CircleCircleIntersection(double R_d, double r_d, +boost::optional circleCircleIntersection(double R_d, double r_d, double tol) { double R2_d2 = R_d*R_d; // Yes, RD-D2 ! @@ -84,11 +106,11 @@ boost::optional Point2::CircleCircleIntersection(double R_d, double r_d, // Hence, there are only solutions if >=0 if (h2<-tol) return boost::none; // allow *slightly* negative else if (h2 Point2::CircleCircleIntersection(Point2 c1, Point2 c2, +list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh) { list solutions; @@ -117,27 +139,21 @@ list Point2::CircleCircleIntersection(Point2 c1, Point2 c2, } /* ************************************************************************* */ -list Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, +list circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) { // distance between circle centers. - double d = c1.dist(c2); + double d = distance2(c1, c2); // centers coincide, either no solution or infinite number of solutions. if (d<1e-9) return list(); // Calculate f and h given normalized radii double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d; - boost::optional fh = CircleCircleIntersection(R_d,r_d); + boost::optional fh = circleCircleIntersection(R_d,r_d); // Call version that takes fh - return CircleCircleIntersection(c1, c2, fh); -} - -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Point2& p) { - os << '(' << p.x() << ", " << p.y() << ')'; - return os; + return circleCircleIntersection(c1, c2, fh); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 3099a8bb3..fb250df6d 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -22,6 +22,14 @@ namespace gtsam { +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS + + /// As of GTSAM 4, in order to make GTSAM more lean, + /// it is now possible to just typedef Point2 to Vector2 + typedef Vector2 Point2; + +#else + /** * A 2D point * Complies with the Testable Concept @@ -29,69 +37,30 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Point2 { +class GTSAM_EXPORT Point2 : public Vector2 { private: - double x_, y_; - public: enum { dimension = 2 }; /// @name Standard Constructors /// @{ /// default constructor - Point2(): x_(0), y_(0) {} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + // Deprecated default constructor initializes to zero, in contrast to new behavior below + Point2() { setZero(); } +#else + Point2() {} +#endif - /// construct from doubles - Point2(double x, double y): x_(x), y_(y) {} + using Vector2::Vector2; /// @} /// @name Advanced Constructors /// @{ /// construct from 2D vector - explicit Point2(const Vector2& v) { - x_ = v(0); - y_ = v(1); - } - - /* - * @brief Circle-circle intersection, given normalized radii. - * Calculate f and h, respectively the parallel and perpendicular distance of - * the intersections of two circles along and from the line connecting the centers. - * Both are dimensionless fractions of the distance d between the circle centers. - * If the circles do not intersect or they are identical, returns boost::none. - * If one solution (touching circles, as determined by tol), h will be exactly zero. - * h is a good measure for how accurate the intersection will be, as when circles touch - * or nearly touch, the intersection is ill-defined with noisy radius measurements. - * @param R_d : R/d, ratio of radius of first circle to distance between centers - * @param r_d : r/d, ratio of radius of second circle to distance between centers - * @param tol: absolute tolerance below which we consider touching circles - * @return optional Point2 with f and h, boost::none if no solution. - */ - static boost::optional CircleCircleIntersection(double R_d, double r_d, - double tol = 1e-9); - - /* - * @brief Circle-circle intersection, from the normalized radii solution. - * @param c1 center of first circle - * @param c2 center of second circle - * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. - */ - static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional); - - /** - * @brief Intersect 2 circles - * @param c1 center of first circle - * @param r1 radius of first circle - * @param c2 center of second circle - * @param r2 radius of second circle - * @param tol: absolute tolerance below which we consider touching circles - * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. - */ - static std::list CircleCircleIntersection(Point2 c1, double r1, - Point2 c2, double r2, double tol = 1e-9); - + explicit Point2(const Vector2& v):Vector2(v) {} /// @} /// @name Testable /// @{ @@ -107,21 +76,7 @@ public: /// @{ /// identity - inline static Point2 identity() {return Point2();} - - /// inverse - inline Point2 operator- () const {return Point2(-x_,-y_);} - - /// add vector on right - inline Point2 operator +(const Vector2& v) const { - return Point2(x_ + v[0], y_ + v[1]); - } - - /// add - inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - - /// subtract - inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + inline static Point2 identity() {return Point2(0,0);} /// @} /// @name Vector Space @@ -137,51 +92,44 @@ public: double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, OptionalJacobian<1,2> H2 = boost::none) const; - /** @deprecated The following function has been deprecated, use distance above */ - inline double dist(const Point2& p2) const { - return (p2 - *this).norm(); - } - - /// multiply with a scalar - inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} - - /// divide by a scalar - inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - /// @} /// @name Standard Interface /// @{ /// equality - inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;} + inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();} /// get x - double x() const {return x_;} + inline double x() const {return (*this)[0];} /// get y - double y() const {return y_;} + inline double y() const {return (*this)[1];} - /// return vectorized form (column-wise). TODO: why does this function exist? - Vector2 vector() const { return Vector2(x_, y_); } + /// return vectorized form (column-wise). + const Vector2& vector() const { return *this; } /// @} - /// @name Deprecated - /// @{ - inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} - inline void operator *= (double s) {x_*=s;y_*=s;} - Point2 inverse() const { return -(*this);} - Point2 compose(const Point2& q) const { return (*this)+q;} - Point2 between(const Point2& q) const { return q-(*this);} - Vector2 localCoordinates(const Point2& q) const { return between(q).vector();} - Point2 retract(const Vector2& v) const { return compose(Point2(v));} - static Vector2 Logmap(const Point2& p) { return p.vector();} - static Point2 Expmap(const Vector2& v) { return Point2(v);} - /// @} - /// Streaming GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + Point2 inverse() const { return -(*this); } + Point2 compose(const Point2& q) const { return (*this)+q;} + Point2 between(const Point2& q) const { return q-(*this);} + Vector2 localCoordinates(const Point2& q) const { return between(q);} + Point2 retract(const Vector2& v) const { return compose(Point2(v));} + static Vector2 Logmap(const Point2& p) { return p;} + static Point2 Expmap(const Vector2& v) { return Point2(v);} + inline double dist(const Point2& p2) const {return distance(p2);} + static boost::optional CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9); + static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9); + /// @} +#endif + private: /// @name Advanced Interface @@ -192,13 +140,25 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(x_); - ar & BOOST_SERIALIZATION_NVP(y_); - } + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector2);} - /// @} + /// @} }; +template<> +struct traits : public internal::VectorSpace { +}; + +#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS + +/// Distance of the point from the origin, with Jacobian +double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); + +/// distance between two points +double distance2(const Point2& p1, const Point2& q, + OptionalJacobian<1, 2> H1 = boost::none, + OptionalJacobian<1, 2> H2 = boost::none); + // Convenience typedef typedef std::pair Point2Pair; std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); @@ -207,10 +167,45 @@ std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); typedef std::vector Point2Vector; /// multiply with scalar -inline Point2 operator*(double s, const Point2& p) {return p*s;} +inline Point2 operator*(double s, const Point2& p) { +return p * s; +} -template<> -struct traits : public internal::VectorSpace {}; +/* + * @brief Circle-circle intersection, given normalized radii. + * Calculate f and h, respectively the parallel and perpendicular distance of + * the intersections of two circles along and from the line connecting the centers. + * Both are dimensionless fractions of the distance d between the circle centers. + * If the circles do not intersect or they are identical, returns boost::none. + * If one solution (touching circles, as determined by tol), h will be exactly zero. + * h is a good measure for how accurate the intersection will be, as when circles touch + * or nearly touch, the intersection is ill-defined with noisy radius measurements. + * @param R_d : R/d, ratio of radius of first circle to distance between centers + * @param r_d : r/d, ratio of radius of second circle to distance between centers + * @param tol: absolute tolerance below which we consider touching circles + * @return optional Point2 with f and h, boost::none if no solution. + */ +boost::optional circleCircleIntersection(double R_d, double r_d, double tol = 1e-9); + +/* + * @brief Circle-circle intersection, from the normalized radii solution. + * @param c1 center of first circle + * @param c2 center of second circle + * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. + */ +std::list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + +/** + * @brief Intersect 2 circles + * @param c1 center of first circle + * @param r1 radius of first circle + * @param c2 center of second circle + * @param r2 radius of second circle + * @param tol: absolute tolerance below which we consider touching circles + * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. + */ +std::list circleCircleIntersection(Point2 c1, double r1, + Point2 c2, double r2, double tol = 1e-9); } // \ namespace gtsam diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index df0f78283..8df5f5607 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -16,12 +16,13 @@ #include #include +#include using namespace std; namespace gtsam { -#ifndef GTSAM_USE_VECTOR3_POINTS +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS bool Point3::equals(const Point3 &q, double tol) const { return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); @@ -34,11 +35,11 @@ void Point3::print(const string& s) const { /* ************************************************************************* */ double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { - return gtsam::distance(*this,q,H1,H2); + return gtsam::distance3(*this,q,H1,H2); } double Point3::norm(OptionalJacobian<1,3> H) const { - return gtsam::norm(*this, H); + return gtsam::norm3(*this, H); } Point3 Point3::normalized(OptionalJacobian<3,3> H) const { @@ -57,7 +58,7 @@ double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1, /* ************************************************************************* */ ostream &operator<<(ostream &os, const Point3& p) { - os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';"; + os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]'"; return os; } @@ -80,8 +81,8 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, #endif /* ************************************************************************* */ -double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, - OptionalJacobian<1, 3> H2) { +double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { double d = (q - p1).norm(); if (H1) { *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z(); @@ -94,7 +95,7 @@ double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, return d; } -double norm(const Point3 &p, OptionalJacobian<1, 3> H) { +double norm3(const Point3 &p, OptionalJacobian<1, 3> H) { double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z()); if (H) { if (fabs(r) > 1e-10) @@ -106,7 +107,7 @@ double norm(const Point3 &p, OptionalJacobian<1, 3> H) { } Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) { - Point3 normalized = p / norm(p); + Point3 normalized = p / p.norm(); if (H) { // 3*3 Derivative double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index fd254e51c..99cb6c2e7 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -29,7 +29,7 @@ namespace gtsam { -#ifdef GTSAM_USE_VECTOR3_POINTS +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point3 to Vector3 @@ -51,8 +51,8 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @name Standard Constructors /// @{ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 // Deprecated default constructor initializes to zero, in contrast to new behavior below +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 Point3() { setZero(); } #endif @@ -124,9 +124,9 @@ class GTSAM_EXPORT Point3 : public Vector3 { Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} - Vector3 localCoordinates(const Point3& q) const { return between(q).vector();} + Vector3 localCoordinates(const Point3& q) const { return between(q);} Point3 retract(const Vector3& v) const { return compose(Point3(v));} - static Vector3 Logmap(const Point3& p) { return p.vector();} + static Vector3 Logmap(const Point3& p) { return p;} static Point3 Expmap(const Vector3& v) { return Point3(v);} inline double dist(const Point3& q) const { return (q - *this).norm(); } Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);} @@ -153,19 +153,19 @@ struct traits : public internal::VectorSpace {}; template<> struct traits : public internal::VectorSpace {}; -#endif +#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS // Convenience typedef typedef std::pair Point3Pair; std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); /// distance between two points -double distance(const Point3& p1, const Point3& q, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none); +double distance3(const Point3& p1, const Point3& q, + OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none); /// Distance of the point from the origin, with Jacobian -double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none); +double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none); /// normalize, with optional Jacobian Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); @@ -180,10 +180,6 @@ double dot(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H_p = boost::none, OptionalJacobian<1, 3> H_q = boost::none); -// Convenience typedef -typedef std::pair Point3Pair; -std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); - template struct Range; @@ -193,7 +189,7 @@ struct Range { double operator()(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) { - return distance(p, q, H1, H2); + return distance3(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 8a0e8fbf5..2a52e98ba 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -19,8 +19,6 @@ #include #include -#include - #include #include #include @@ -55,21 +53,21 @@ void Pose2::print(const string& s) const { /* ************************************************************************* */ bool Pose2::equals(const Pose2& q, double tol) const { - return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); + return equal_with_abs_tol(t_, q.t_, tol) && r_.equals(q.r_, tol); } /* ************************************************************************* */ Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) { - if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); - Point2 v(xi(0),xi(1)); - double w = xi(2); + if (H) *H = Pose2::ExpmapDerivative(xi); + const Point2 v(xi(0),xi(1)); + const double w = xi(2); if (std::abs(w) < 1e-10) return Pose2(xi[0], xi[1], xi[2]); else { - Rot2 R(Rot2::fromAngle(w)); - Point2 v_ortho = R_PI_2 * v; // points towards rot center - Point2 t = (v_ortho - R.rotate(v_ortho)) / w; + const Rot2 R(Rot2::fromAngle(w)); + const Point2 v_ortho = R_PI_2 * v; // points towards rot center + const Point2 t = (v_ortho - R.rotate(v_ortho)) / w; return Pose2(R, t); } } @@ -251,7 +249,7 @@ double Pose2::range(const Point2& point, Point2 d = point - t_; if (!Hpose && !Hpoint) return d.norm(); Matrix12 D_r_d; - double r = d.norm(D_r_d); + double r = norm2(d, D_r_d); if (Hpose) { Matrix23 D_d_pose; D_d_pose << -r_.c(), r_.s(), 0.0, @@ -269,7 +267,7 @@ double Pose2::range(const Pose2& pose, Point2 d = pose.t() - t_; if (!Hpose && !Hother) return d.norm(); Matrix12 D_r_d; - double r = d.norm(D_r_d); + double r = norm2(d, D_r_d); if (Hpose) { Matrix23 D_d_pose; D_d_pose << @@ -313,8 +311,8 @@ boost::optional align(const vector& pairs) { if (n<2) return boost::none; // we need at least two pairs // calculate centroids - Point2 cp,cq; - BOOST_FOREACH(const Point2Pair& pair, pairs) { + Point2 cp(0,0), cq(0,0); + for(const Point2Pair& pair: pairs) { cp += pair.first; cq += pair.second; } @@ -323,7 +321,7 @@ boost::optional align(const vector& pairs) { // calculate cos and sin double c=0,s=0; - BOOST_FOREACH(const Point2Pair& pair, pairs) { + for(const Point2Pair& pair: pairs) { Point2 dq = pair.first - cp; Point2 dp = pair.second - cq; c += dp.x() * dq.x() + dp.y() * dq.y(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 31dfb479f..1ba384857 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -52,7 +52,9 @@ public: /// @{ /** default constructor = origin */ - Pose2() {} // default is origin + Pose2() : + r_(traits::Identity()), t_(traits::Identity()) { + } /** copy constructor */ Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} @@ -86,7 +88,7 @@ public: /// @{ /** Construct from canonical coordinates \f$ [T_x,T_y,\theta] \f$ (Lie algebra) */ - Pose2(const Vector& v) { + Pose2(const Vector& v) : Pose2() { *this = Expmap(v); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index d170282fe..50e487e75 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #include @@ -346,7 +345,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, return local.norm(); } else { Matrix13 D_r_local; - const double r = norm(local, D_r_local); + const double r = norm3(local, D_r_local); if (H1) *H1 = D_r_local * D_local_pose; if (H2) *H2 = D_r_local * D_local_point; return r; @@ -379,6 +378,16 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, } } +/* ************************************************************************* */ +Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1, + OptionalJacobian<2, 6> H2) const { + if (H2) { + H2->setZero(); + return bearing(pose.translation(), H1, H2.cols<3>(3)); + } + return bearing(pose.translation(), H1, boost::none); +} + /* ************************************************************************* */ boost::optional Pose3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); @@ -424,7 +433,7 @@ boost::optional Pose3::Align(const std::vector& abPointPairs) boost::optional align(const vector& baPointPairs) { vector abPointPairs; - BOOST_FOREACH (const Point3Pair& baPair, baPointPairs) { + for (const Point3Pair& baPair: baPointPairs) { abPointPairs.push_back(make_pair(baPair.second, baPair.first)); } return Pose3::Align(abPointPairs); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ee1206119..6df62485b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -282,6 +282,15 @@ public: Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none, OptionalJacobian<2, 3> H2 = boost::none) const; + /** + * Calculate bearing to another pose + * @param other 3D location and orientation of other body. The orientation + * information is ignored. + * @return bearing (Unit3) + */ + Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> H1 = boost::none, + OptionalJacobian<2, 6> H2 = boost::none) const; + /// @} /// @name Advanced Interface /// @{ @@ -353,6 +362,9 @@ struct traits : public internal::LieGroup {}; template <> struct Bearing : HasBearing {}; +template<> +struct Bearing : HasBearing {}; + template struct Range : HasRange {}; diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 0ab4a5ee6..af9481181 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,6 +21,7 @@ #include #include // Logmap/Expmap derivatives #include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 9780cb49a..4cabe7140 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -17,6 +17,7 @@ */ #include +#include using namespace std; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 79b13b93e..bb0278953 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -118,6 +118,7 @@ namespace gtsam { * @param q The quaternion */ Rot3(const Quaternion& q); + Rot3(double x, double y, double z, double w) : Rot3(Quaternion(x, y, z, w)) {} /// Random, generates a random axis, then random angle \in [-p,pi] static Rot3 Random(boost::mt19937 & rng); @@ -156,12 +157,17 @@ namespace gtsam { /** * Returns rotation nRb from body to nav frame. + * For vehicle coordinate frame X forward, Y right, Z down: * Positive yaw is to right (as in aircraft heading). * Positive pitch is up (increasing aircraft altitude). * Positive roll is to right (increasing yaw in aircraft). * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf. - * Assumes vehicle coordinate frame X forward, Y right, Z down. + * + * For vehicle coordinate frame X forward, Y left, Z up: + * Positive yaw is to left (as in aircraft heading). + * Positive pitch is down (decreasing aircraft altitude). + * Positive roll is to right (decreasing yaw in aircraft). */ static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);} @@ -179,7 +185,7 @@ namespace gtsam { */ static Rot3 AxisAngle(const Point3& axis, double angle) { #ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis.vector())); + return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); #else return Rot3(SO3::AxisAngle(axis,angle)); #endif diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index f8a01141b..8af9a7144 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -92,7 +92,7 @@ namespace gtsam { const Matrix3 R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R; - const Vector3 r = R * p.vector(); + const Vector3 r = R * p; return Point3(r.x(), r.y(), r.z()); } diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index bd111d9b1..07c03ab49 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -116,6 +117,12 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { return so3::ExpmapFunctor(axis, theta).expmap(); } +/* ************************************************************************* */ +void SO3::print(const std::string& s) const { + std::cout << s << *this << std::endl; + } + +/* ************************************************************************* */ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { so3::DexpFunctor impl(omega); diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 3152fa2fb..53f2c2130 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { @@ -67,9 +68,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s) const { - std::cout << s << *this << std::endl; - } + void print(const std::string& s) const; bool equals(const SO3 & R, double tol) const { return equal_with_abs_tol(*this, R, tol); diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 5c6646454..c34d4d8f3 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -39,7 +39,10 @@ namespace gtsam { const Point3 q = leftCamPose_.transform_to(point); - if ( q.z() <= 0 ) throw StereoCheiralityException(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw StereoCheiralityException(); +#endif // get calibration const Cal3_S2Stereo& K = *K_; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 9f21bb82a..cd05af519 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -78,7 +78,7 @@ public: /// Construct from 2D point in plane at focal length f /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point - explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) { + explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) { p_.normalize(); } diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index e9eb689e1..8de049fa4 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -52,7 +52,7 @@ TEST( Cal3Bundler, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( pn.equals(pn_hat, 1e-5)); + CHECK( traits::Equals(pn, pn_hat, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index c5a6be2d6..416665d46 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -48,7 +48,7 @@ TEST( Cal3DS2, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( pn.equals(pn_hat, 1e-5)); + CHECK( traits::Equals(pn, pn_hat, 1e-5)); } Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); } diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index de9a8b739..2c5ffd7fb 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -59,7 +59,7 @@ TEST( Cal3Unified, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); - CHECK( p.equals(pn_hat, 1e-8)); + CHECK( traits::Equals(p, pn_hat, 1e-8)); } Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index cc7a0c0f8..e629eb3c6 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -30,10 +30,10 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) // Camera situated at 0.5 meters high, looking down -static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), +static const Pose3 kDefaultPose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); -static const CalibratedCamera camera(pose1); +static const CalibratedCamera camera(kDefaultPose); static const Point3 point1(-0.08,-0.08, 0.0); static const Point3 point2(-0.08, 0.08, 0.0); @@ -43,7 +43,19 @@ static const Point3 point4( 0.08,-0.08, 0.0); /* ************************************************************************* */ TEST( CalibratedCamera, constructor) { - CHECK(assert_equal( camera.pose(), pose1)); + CHECK(assert_equal( camera.pose(), kDefaultPose)); +} + +//****************************************************************************** +TEST(CalibratedCamera, Create) { + Matrix actualH; + EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH))); + + // Check derivative + boost::function f = // + boost::bind(CalibratedCamera::Create, _1, boost::none); + Matrix numericalH = numericalDerivative11(f, kDefaultPose); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); } /* ************************************************************************* */ @@ -131,8 +143,8 @@ TEST( CalibratedCamera, Dproject_point_pose) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2) { - static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); - static const CalibratedCamera camera(pose1); + static const Pose3 kDefaultPose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const CalibratedCamera camera(kDefaultPose); Matrix Dpose, Dpoint; camera.project(point1, Dpose, Dpoint); Matrix numerical_pose = numericalDerivative21(project2, camera, point1); @@ -152,7 +164,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); - CHECK(assert_equal(Point2(), result)); + CHECK(assert_equal(Point2(0,0), result)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } @@ -161,8 +173,8 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2_infinity) { - static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); - static const CalibratedCamera camera(pose1); + static const Pose3 pose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose); Matrix Dpose, Dpoint; camera.project2(pointAtInfinity, Dpose, Dpoint); Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 0afa04411..01f784ae0 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -44,7 +44,7 @@ TEST(CameraSet, Pinhole) { EXPECT(!set.equals(set2)); // Check measurements - Point2 expected; + Point2 expected(0,0); ZZ z = set.project2(p); EXPECT(assert_equal(expected, z[0])); EXPECT(assert_equal(expected, z[1])); @@ -90,7 +90,7 @@ TEST(CameraSet, Pinhole) { Vector v = Ft * (b - E * P * Et * b); schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30; SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b); - EXPECT(assert_equal(schur, actualReduced.matrix())); + EXPECT(assert_equal(schur, actualReduced.selfadjointView())); // Check Schur complement update, same order, should just double FastVector allKeys, keys; @@ -99,7 +99,7 @@ TEST(CameraSet, Pinhole) { keys.push_back(1); keys.push_back(2); Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced); - EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.matrix())); + EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView())); // Check Schur complement update, keys reversed FastVector keys2; @@ -111,13 +111,13 @@ TEST(CameraSet, Pinhole) { Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b); Matrix A(19, 19); A << Ft * F - Ft * E * P * Et * F, reverse_v, reverse_v.transpose(), 30; - EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix())); + EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.selfadjointView())); // reprojectionErrorAtInfinity Unit3 pointAtInfinity(0, 0, 1000); EXPECT( assert_equal(pointAtInfinity, - camera.backprojectPointAtInfinity(Point2()))); + camera.backprojectPointAtInfinity(Point2(0,0)))); actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); EXPECT(assert_equal(expectedV, actualV)); LONGS_EQUAL(2, Fs.size()); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 99dcb95bf..a9b68bdec 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -153,12 +153,12 @@ TEST( PinholeCamera, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x.first)); + EXPECT(assert_equal(Point2(0,0), x.first)); EXPECT(x.second); } @@ -169,12 +169,12 @@ TEST( PinholeCamera, backprojectInfinity2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0)); Unit3 expected(0., 1., 0.); Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x)); + EXPECT(assert_equal(Point2(0,0), x)); } /* ************************************************************************* */ @@ -184,12 +184,12 @@ TEST( PinholeCamera, backprojectInfinity3) Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); - Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0)); Unit3 expected(0., 0., 1.); Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x)); + EXPECT(assert_equal(Point2(0,0), x)); } /* ************************************************************************* */ @@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) { double result = camera.range(point1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, + EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(Hexpected1, D1, 1e-7)); EXPECT(assert_equal(Hexpected2, D2, 1e-7)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 0d840de7e..ecbb92061 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -124,12 +124,12 @@ TEST( PinholePose, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x.first)); + EXPECT(assert_equal(Point2(0,0), x.first)); EXPECT(x.second); } @@ -212,7 +212,7 @@ TEST( PinholePose, range0) { double result = camera.range(point1, D1, D2); Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); + EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index b8f001f1c..28b7ddac6 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -84,7 +84,7 @@ TEST(PinholeSet, Pinhole) { EXPECT(!set.equals(set2)); // Check measurements - Point2 expected; + Point2 expected(0,0); ZZ z = set.project2(p); EXPECT(assert_equal(expected, z[0])); EXPECT(assert_equal(expected, z[1])); @@ -131,7 +131,7 @@ TEST(PinholeSet, Pinhole) { } EXPECT( assert_equal(pointAtInfinity, - camera.backprojectPointAtInfinity(Point2()))); + camera.backprojectPointAtInfinity(Point2(0,0)))); { PinholeSet::FBlocks Fs; Matrix E; diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 3a636b9bf..e2a5bcdea 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -27,6 +27,11 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2) +//****************************************************************************** +TEST(Point2 , Constructor) { + Point2 p; +} + //****************************************************************************** TEST(Double , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -95,26 +100,26 @@ TEST( Point2, expmap) { /* ************************************************************************* */ TEST( Point2, arithmetic) { - EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); - EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); - EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); - EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); - EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); - EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); + EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); + EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); + EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); + EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); + EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); + EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); } /* ************************************************************************* */ TEST( Point2, unit) { Point2 p0(10, 0), p1(0, -10), p2(10, 10); - EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6)); - EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6)); - EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6)); + EXPECT(assert_equal(Point2(1, 0), Point2(p0.normalized()), 1e-6)); + EXPECT(assert_equal(Point2(0,-1), Point2(p1.normalized()), 1e-6)); + EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), Point2(p2.normalized()), 1e-6)); } namespace { /* ************************************************************************* */ // some shared test values - Point2 x1, x2(1, 1), x3(1, 1); + Point2 x1(0,0), x2(1, 1), x3(1, 1); Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); /* ************************************************************************* */ @@ -126,19 +131,19 @@ TEST( Point2, norm ) { Point2 p0(cos(5.0), sin(5.0)); DOUBLES_EQUAL(1, p0.norm(), 1e-6); Point2 p1(4, 5), p2(1, 1); - DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6); + DOUBLES_EQUAL( 5, distance2(p1, p2), 1e-6); DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6); Matrix expectedH, actualH; double actual; // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1] - actual = x1.norm(actualH); + actual = norm2(x1, actualH); EXPECT_DOUBLES_EQUAL(0, actual, 1e-9); expectedH = (Matrix(1, 2) << 1.0, 1.0).finished(); EXPECT(assert_equal(expectedH,actualH)); - actual = x2.norm(actualH); + actual = norm2(x2, actualH); EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9); expectedH = numericalDerivative11(norm_proxy, x2); EXPECT(assert_equal(expectedH,actualH)); @@ -151,20 +156,20 @@ TEST( Point2, norm ) { /* ************************************************************************* */ namespace { double distance_proxy(const Point2& location, const Point2& point) { - return location.distance(point); + return distance2(location, point); } } TEST( Point2, distance ) { Matrix expectedH1, actualH1, expectedH2, actualH2; // establish distance is indeed zero - EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9); + EXPECT_DOUBLES_EQUAL(1, distance2(x1, l1), 1e-9); // establish distance is indeed 45 degrees - EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9); + EXPECT_DOUBLES_EQUAL(sqrt(2.0), distance2(x1, l2), 1e-9); // Another pair - double actual23 = x2.distance(l3, actualH1, actualH2); + double actual23 = distance2(x2, l3, actualH1, actualH2); EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9); // Check numerical derivatives @@ -174,7 +179,7 @@ TEST( Point2, distance ) { EXPECT(assert_equal(expectedH2,actualH2)); // Another test - double actual34 = x3.distance(l4, actualH1, actualH2); + double actual34 = distance2(x3, l4, actualH1, actualH2); EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9); // Check numerical derivatives @@ -190,42 +195,42 @@ TEST( Point2, circleCircleIntersection) { double offset = 0.994987; // Test intersections of circle moving from inside to outside - list inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1); + list inside = circleCircleIntersection(Point2(0,0),5,Point2(0,0),1); EXPECT_LONGS_EQUAL(0,inside.size()); - list touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1); + list touching1 = circleCircleIntersection(Point2(0,0),5,Point2(4,0),1); EXPECT_LONGS_EQUAL(1,touching1.size()); EXPECT(assert_equal(Point2(5,0), touching1.front())); - list common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1); + list common = circleCircleIntersection(Point2(0,0),5,Point2(5,0),1); EXPECT_LONGS_EQUAL(2,common.size()); EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6)); EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6)); - list touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1); + list touching2 = circleCircleIntersection(Point2(0,0),5,Point2(6,0),1); EXPECT_LONGS_EQUAL(1,touching2.size()); EXPECT(assert_equal(Point2(5,0), touching2.front())); // test rotated case - list rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1); + list rotated = circleCircleIntersection(Point2(0,0),5,Point2(0,5),1); EXPECT_LONGS_EQUAL(2,rotated.size()); EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6)); EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6)); // test r1 smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5); + list smaller = circleCircleIntersection(Point2(0,0),1,Point2(5,0),5); EXPECT_LONGS_EQUAL(2,smaller.size()); EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6)); EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6)); // test offset case, r1>r2 - list offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1); + list offset1 = circleCircleIntersection(Point2(1,1),5,Point2(6,1),1); EXPECT_LONGS_EQUAL(2,offset1.size()); EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6)); EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6)); // test offset case, r1 offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5); + list offset2 = circleCircleIntersection(Point2(6,1),1,Point2(1,1),5); EXPECT_LONGS_EQUAL(2,offset2.size()); EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6)); EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6)); @@ -233,12 +238,14 @@ TEST( Point2, circleCircleIntersection) { } /* ************************************************************************* */ +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS TEST( Point2, stream) { Point2 p(1, 2); std::ostringstream os; os << p; EXPECT(os.str() == "(1, 2)"); } +#endif /* ************************************************************************* */ int main () { diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 9b6e53323..e2396f7e9 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -26,6 +26,11 @@ GTSAM_CONCEPT_LIE_INST(Point3) static Point3 P(0.2, 0.7, -2); +//****************************************************************************** +TEST(Point3 , Constructor) { + Point3 p; +} + //****************************************************************************** TEST(Point3 , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -149,12 +154,12 @@ TEST( Point3, cross2) { } /* ************************************************************************* */ -#ifndef GTSAM_USE_VECTOR3_POINTS +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS TEST( Point3, stream) { Point3 p(1, 2, -3); std::ostringstream os; os << p; - EXPECT(os.str() == "[1, 2, -3]';"); + EXPECT(os.str() == "[1, 2, -3]'"); } #endif @@ -178,20 +183,20 @@ TEST (Point3, norm) { Matrix actualH; Point3 point(3,4,5); // arbitrary point double expected = sqrt(50); - EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8); + EXPECT_DOUBLES_EQUAL(expected, norm3(point, actualH), 1e-8); Matrix expectedH = numericalDerivative11(norm_proxy, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } /* ************************************************************************* */ double testFunc(const Point3& P, const Point3& Q) { - return distance(P,Q); + return distance3(P, Q); } TEST (Point3, distance) { Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3); Matrix H1, H2; - double d = distance(P, Q, H1, H2); + double d = distance3(P, Q, H1, H2); double expectedDistance = 55.542686; Matrix numH1 = numericalDerivative21(testFunc, P, Q); Matrix numH2 = numericalDerivative22(testFunc, P, Q); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index d1f68fe28..10fd431bc 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include // for operator += #include @@ -44,7 +43,7 @@ TEST(Pose2 , Concept) { /* ************************************************************************* */ TEST(Pose2, constructors) { - Point2 p; + Point2 p(0,0); Pose2 pose(0,p); Pose2 origin; assert_equal(pose,origin); @@ -372,7 +371,7 @@ TEST(Pose2, compose_c) /* ************************************************************************* */ TEST(Pose2, inverse ) { - Point2 origin, t(1,2); + Point2 origin(0,0), t(1,2); Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y Pose2 identity, lTg = gTl.inverse(); @@ -410,7 +409,7 @@ namespace { /* ************************************************************************* */ TEST( Pose2, matrix ) { - Point2 origin, t(1,2); + Point2 origin(0,0), t(1,2); Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y Matrix gMl = matrix(gTl); EXPECT(assert_equal((Matrix(3,3) << @@ -744,7 +743,7 @@ namespace { /* ************************************************************************* */ struct Triangle { size_t i_,j_,k_;}; - boost::optional align(const vector& ps, const vector& qs, + boost::optional align2(const vector& ps, const vector& qs, const pair& trianglePair) { const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; vector correspondences; @@ -763,7 +762,7 @@ TEST(Pose2, align_4) { Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2; Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0; - boost::optional actual = align(ps, qs, make_pair(t1,t2)); + boost::optional actual = align2(ps, qs, make_pair(t1,t2)); EXPECT(assert_equal(expected, *actual)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 97ccbcb34..98d3e11ee 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -708,6 +708,27 @@ TEST(Pose3, Bearing2) { EXPECT(assert_equal(expectedH2, actualH2)); } +TEST(Pose3, PoseToPoseBearing) { + Matrix expectedH1, actualH1, expectedH2, actualH2, H2block; + EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2); + + // Since the second pose is treated as a point, the value calculated by + // numericalDerivative22 only depends on the position of the pose. Here, we + // calculate the Jacobian w.r.t. the second pose's position, and then augment + // that with zeroes in the block that is w.r.t. the second pose's + // orientation. + H2block = numericalDerivative22(bearing_proxy, xl1, l2); + expectedH2 = Matrix(2, 6); + expectedH2.setZero(); + expectedH2.block<2, 3>(0, 3) = H2block; + + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ TEST( Pose3, unicycle ) { diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 5e72d4c5b..6f0af3828 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -231,9 +231,9 @@ TEST(Rot3, retract_localCoordinates) /* ************************************************************************* */ TEST(Rot3, expmap_logmap) { - Vector3 d12 = Vector3::Constant(0.1); + Vector d12 = Vector3::Constant(0.1); Rot3 R2 = R.expmap(d12); - EXPECT(assert_equal(d12, (Vector) R.logmap(R2))); + EXPECT(assert_equal(d12, R.logmap(R2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 70b3069f2..edf122d3c 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -104,12 +104,12 @@ TEST( SimpleCamera, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down SimpleCamera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x.first)); + CHECK(assert_equal(Point2(0,0), x.first)); CHECK(x.second); } diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 497599a6a..bc432cad3 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -104,6 +104,23 @@ TEST( StereoCamera, Dproject) CHECK(assert_equal(expected2,actual2,1e-7)); } +/* ************************************************************************* */ +TEST( StereoCamera, projectCheirality) +{ + // create a Stereo camera + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam(Pose3(), K); + + // point behind the camera + Point3 p(0, 0, -5); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException); +#else // otherwise project should not throw the exception + StereoPoint2 expected = StereoPoint2(320, 470, 240); + CHECK(assert_equal(expected,stereoCam.project2(p),1e-7)); +#endif +} + /* ************************************************************************* */ TEST( StereoCamera, backproject_case1) { diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index c3df95abc..9e599f3f5 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -246,6 +246,59 @@ TEST( triangulation, fourPoses_distinct_Ks) { #endif } +//****************************************************************************** +TEST( triangulation, outliersAndFarLandmarks) { + Cal3_S2 K1(1500, 1200, 0, 640, 480); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + SimpleCamera camera1(pose1, K1); + + // create second camera 1 meter to the right of first camera + Cal3_S2 K2(1600, 1300, 0, 650, 440); + SimpleCamera camera2(pose2, K2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 z1 = camera1.project(landmark); + Point2 z2 = camera2.project(landmark); + + vector cameras; + vector measurements; + + cameras += camera1, camera2; + measurements += z1, z2; + + double landmarkDistanceThreshold = 10; // landmark is closer than that + TriangulationParameters params(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold + TriangulationResult actual = triangulateSafe(cameras,measurements,params); + EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(actual.valid()); + + landmarkDistanceThreshold = 4; // landmark is farther than that + TriangulationParameters params2(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold + actual = triangulateSafe(cameras,measurements,params2); + EXPECT(actual.farPoint()); + + // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER) + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Cal3_S2 K3(700, 500, 0, 640, 480); + SimpleCamera camera3(pose3, K3); + Point2 z3 = camera3.project(landmark); + + cameras += camera3; + measurements += z3 + Point2(10, -10); + + landmarkDistanceThreshold = 10; // landmark is closer than that + double outlierThreshold = 100; // loose, the outlier is going to pass + TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,outlierThreshold); + actual = triangulateSafe(cameras,measurements,params3); + EXPECT(actual.valid()); + + // now set stricter threshold for outlier rejection + outlierThreshold = 5; // tighter, the outlier is not going to pass + TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,outlierThreshold); + actual = triangulateSafe(cameras,measurements,params4); + EXPECT(actual.outlier()); +} + //****************************************************************************** TEST( triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) @@ -273,7 +326,7 @@ TEST( triangulation, onePose) { vector measurements; poses += Pose3(); - measurements += Point2(); + measurements += Point2(0,0); CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), TriangulationUnderconstrainedException); @@ -282,7 +335,7 @@ TEST( triangulation, onePose) { //****************************************************************************** TEST( triangulation, StereotriangulateNonlinear ) { - Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + auto stereoK = boost::make_shared(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); // two camera poses m1, m2 Matrix4 m1, m2; @@ -325,12 +378,12 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK))); - graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK))); + graph.emplace_shared(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK); + graph.emplace_shared(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); - graph.push_back(PriorFactor(Symbol('x',1), Pose3(m1), posePrior)); - graph.push_back(PriorFactor(Symbol('x',2), Pose3(m2), posePrior)); + graph.emplace_shared >(Symbol('x',1), Pose3(m1), posePrior); + graph.emplace_shared >(Symbol('x',2), Pose3(m2), posePrior); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); @@ -346,8 +399,8 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.push_back(TriangulationFactor(cameras[0], measurements[0], unit, Symbol('l',1))); - graph.push_back(TriangulationFactor(cameras[1], measurements[1], unit, Symbol('l',1))); + graph.emplace_shared >(cameras[0], measurements[0], unit, Symbol('l',1)); + graph.emplace_shared >(cameras[1], measurements[1], unit, Symbol('l',1)); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 0e99268ee..71093d668 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -33,7 +33,6 @@ #include #include -#include #include #include #include @@ -56,7 +55,7 @@ TEST(Unit3, point3) { ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) / sqrt(2.0); Matrix actualH, expectedH; - BOOST_FOREACH(Point3 p,ps) { + for(Point3 p: ps) { Unit3 s(p); expectedH = numericalDerivative11(point3_, s); EXPECT(assert_equal(p, s.point3(actualH), 1e-8)); @@ -427,7 +426,7 @@ TEST(Unit3, ErrorBetweenFactor) { // Add prior factors. SharedNoiseModel R_prior = noiseModel::Unit::Create(2); for (size_t i = 0; i < data.size(); i++) { - graph.add(PriorFactor(U(i), data[i], R_prior)); + graph.emplace_shared >(U(i), data[i], R_prior); } // Add process factors using the dot product error function. diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 4e8ae2f17..fdfe32e8b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -89,8 +89,8 @@ std::pair triangulationGraph( const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit2, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit2, landmarkKey); } return std::make_pair(graph, values); } @@ -112,11 +112,12 @@ std::pair triangulationGraph( Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension)); + static SharedNoiseModel unit(noiseModel::Unit::Create( + traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit, landmarkKey); } return std::make_pair(graph, values); } @@ -237,7 +238,7 @@ Point3 triangulatePoint3(const std::vector& poses, // construct projection matrices from poses & calibration std::vector projection_matrices; CameraProjectionMatrix createP(*sharedCal); // partially apply - BOOST_FOREACH(const Pose3& pose, poses) + for(const Pose3& pose: poses) projection_matrices.push_back(createP(pose)); // Triangulate linearly @@ -250,7 +251,7 @@ Point3 triangulatePoint3(const std::vector& poses, #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - BOOST_FOREACH(const Pose3& pose, poses) { + for(const Pose3& pose: poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) throw(TriangulationCheiralityException()); @@ -286,7 +287,7 @@ Point3 triangulatePoint3( // construct projection matrices from poses & calibration std::vector projection_matrices; - BOOST_FOREACH(const CAMERA& camera, cameras) + for(const CAMERA& camera: cameras) projection_matrices.push_back( CameraProjectionMatrix(camera.calibration())( camera.pose())); @@ -298,7 +299,7 @@ Point3 triangulatePoint3( #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - BOOST_FOREACH(const CAMERA& camera, cameras) { + for(const CAMERA& camera: cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) throw(TriangulationCheiralityException()); @@ -321,6 +322,7 @@ Point3 triangulatePoint3( struct TriangulationParameters { double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) bool enableEPI; ///< if set to true, will refine triangulation using LM /** @@ -382,7 +384,7 @@ private: */ class TriangulationResult: public boost::optional { enum Status { - VALID, DEGENERATE, BEHIND_CAMERA + VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; Status status_; TriangulationResult(Status s) : @@ -405,12 +407,27 @@ public: static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } + static TriangulationResult Outlier() { + return TriangulationResult(OUTLIER); + } + static TriangulationResult FarPoint() { + return TriangulationResult(FAR_POINT); + } static TriangulationResult BehindCamera() { return TriangulationResult(BEHIND_CAMERA); } + bool valid() const { + return status_ == VALID; + } bool degenerate() const { return status_ == DEGENERATE; } + bool outlier() const { + return status_ == OUTLIER; + } + bool farPoint() const { + return status_ == FAR_POINT; + } bool behindCamera() const { return status_ == BEHIND_CAMERA; } @@ -453,13 +470,13 @@ TriangulationResult triangulateSafe(const std::vector& cameras, // Check landmark distance and re-projection errors to avoid outliers size_t i = 0; - double totalReprojError = 0.0; - BOOST_FOREACH(const CAMERA& camera, cameras) { + double maxReprojError = 0.0; + for(const CAMERA& camera: cameras) { const Pose3& pose = camera.pose(); if (params.landmarkDistanceThreshold > 0 - && distance(pose.translation(), point) + && distance3(pose.translation(), point) > params.landmarkDistanceThreshold) - return TriangulationResult::Degenerate(); + return TriangulationResult::FarPoint(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras // Only needed if this was not yet handled by exception @@ -471,14 +488,14 @@ TriangulationResult triangulateSafe(const std::vector& cameras, if (params.dynamicOutlierRejectionThreshold > 0) { const Point2& zi = measured.at(i); Point2 reprojectionError(camera.project(point) - zi); - totalReprojError += reprojectionError.vector().norm(); + maxReprojError = std::max(maxReprojError, reprojectionError.norm()); } i += 1; } // Flag as degenerate if average reprojection error is too large if (params.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params.dynamicOutlierRejectionThreshold) - return TriangulationResult::Degenerate(); + && maxReprojError > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Outlier(); // all good! return TriangulationResult(point); diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index f5e69fcce..a3bd87887 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -40,11 +40,11 @@ namespace gtsam { std::ofstream of(s.c_str()); of << "digraph G{\n"; - BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) { + for (auto conditional: boost::adaptors::reverse(*this)) { typename CONDITIONAL::Frontals frontals = conditional->frontals(); Key me = frontals.front(); typename CONDITIONAL::Parents parents = conditional->parents(); - BOOST_FOREACH(Key p, parents) + for(Key p: parents) of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl; } diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 3a3e1317c..e04af1339 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -26,7 +26,6 @@ #include #include -#include #include #include @@ -38,7 +37,7 @@ namespace gtsam { template BayesTreeCliqueData BayesTree::getCliqueData() const { BayesTreeCliqueData data; - BOOST_FOREACH(const sharedClique& root, roots_) + for(const sharedClique& root: roots_) getCliqueData(data, root); return data; } @@ -48,7 +47,7 @@ namespace gtsam { void BayesTree::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const { data.conditionalSizes.push_back(clique->conditional()->nrFrontals()); data.separatorSizes.push_back(clique->conditional()->nrParents()); - BOOST_FOREACH(sharedClique c, clique->children) { + for(sharedClique c: clique->children) { getCliqueData(data, c); } } @@ -57,7 +56,7 @@ namespace gtsam { template size_t BayesTree::numCachedSeparatorMarginals() const { size_t count = 0; - BOOST_FOREACH(const sharedClique& root, roots_) + for(const sharedClique& root: roots_) count += root->numCachedSeparatorMarginals(); return count; } @@ -68,7 +67,7 @@ namespace gtsam { if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); std::ofstream of(s.c_str()); of<< "digraph G{\n"; - BOOST_FOREACH(const sharedClique& root, roots_) + for(const sharedClique& root: roots_) saveGraph(of, root, keyFormatter); of<<"}"; of.close(); @@ -84,7 +83,7 @@ namespace gtsam { std::string parent = out.str(); parent += "[label=\""; - BOOST_FOREACH(Key index, clique->conditional_->frontals()) { + for(Key index: clique->conditional_->frontals()) { if(!first) parent += ","; first = false; parent += indexFormatter(index); } @@ -95,7 +94,7 @@ namespace gtsam { } first = true; - BOOST_FOREACH(Key sep, clique->conditional_->parents()) { + for(Key sep: clique->conditional_->parents()) { if(!first) parent += ","; first = false; parent += indexFormatter(sep); } @@ -103,7 +102,7 @@ namespace gtsam { s << parent; parentnum = num; - BOOST_FOREACH(sharedClique c, clique->children) { + for(sharedClique c: clique->children) { num++; saveGraph(s, c, indexFormatter, parentnum); } @@ -113,7 +112,7 @@ namespace gtsam { template size_t BayesTree::size() const { size_t size = 0; - BOOST_FOREACH(const sharedClique& clique, roots_) + for(const sharedClique& clique: roots_) size += clique->treeSize(); return size; } @@ -121,7 +120,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::addClique(const sharedClique& clique, const sharedClique& parent_clique) { - BOOST_FOREACH(Key j, clique->conditional()->frontals()) + for(Key j: clique->conditional()->frontals()) nodes_[j] = clique; if (parent_clique != NULL) { clique->parent_ = parent_clique; @@ -189,7 +188,7 @@ namespace gtsam { this->clear(); boost::shared_ptr rootContainer = boost::make_shared(); treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre); - BOOST_FOREACH(const sharedClique& root, rootContainer->children) { + for(const sharedClique& root: rootContainer->children) { root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique insertRoot(root); } @@ -234,13 +233,13 @@ namespace gtsam { template void BayesTree::fillNodesIndex(const sharedClique& subtree) { // Add each frontal variable of this root node - BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { + for(const Key& j: subtree->conditional()->frontals()) { bool inserted = nodes_.insert(std::make_pair(j, subtree)).second; assert(inserted); (void)inserted; } // Fill index for each child typedef typename BayesTree::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, subtree->children) { + for(const sharedClique& child: subtree->children) { fillNodesIndex(child); } } @@ -345,7 +344,7 @@ namespace gtsam { boost::shared_ptr p_C1_B; { FastVector C1_minus_B; { KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); - BOOST_FOREACH(const Key j, *B->conditional()) { + for(const Key j: *B->conditional()) { C1_minus_B_set.erase(j); } C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); } @@ -357,7 +356,7 @@ namespace gtsam { boost::shared_ptr p_C2_B; { FastVector C2_minus_B; { KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); - BOOST_FOREACH(const Key j, *B->conditional()) { + for(const Key j: *B->conditional()) { C2_minus_B_set.erase(j); } C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); } @@ -403,7 +402,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::deleteCachedShortcuts() { - BOOST_FOREACH(const sharedClique& root, roots_) { + for(const sharedClique& root: roots_) { root->deleteCachedShortcuts(); } } @@ -424,10 +423,10 @@ namespace gtsam { } // orphan my children - BOOST_FOREACH(sharedClique child, clique->children) + for(sharedClique child: clique->children) child->parent_ = typename Clique::weak_ptr(); - BOOST_FOREACH(Key j, clique->conditional()->frontals()) { + for(Key j: clique->conditional()->frontals()) { nodes_.unsafe_erase(j); } } @@ -462,7 +461,7 @@ namespace gtsam { void BayesTree::removeTop(const FastVector& keys, BayesNetType& bn, Cliques& orphans) { // process each key of the new factor - BOOST_FOREACH(const Key& j, keys) + for(const Key& j: keys) { // get the clique // TODO: Nodes will be searched again in removeClique @@ -475,7 +474,7 @@ namespace gtsam { // Delete cachedShortcuts for each orphan subtree //TODO: Consider Improving - BOOST_FOREACH(sharedClique& orphan, orphans) + for(sharedClique& orphan: orphans) orphan->deleteCachedShortcuts(); } @@ -499,14 +498,14 @@ namespace gtsam { for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique) { // Add children - BOOST_FOREACH(const sharedClique& child, (*clique)->children) { + for(const sharedClique& child: (*clique)->children) { cliques.push_back(child); } // Delete cached shortcuts (*clique)->deleteCachedShortcutsNonRecursive(); // Remove this node from the nodes index - BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) { + for(Key j: (*clique)->conditional()->frontals()) { nodes_.unsafe_erase(j); } // Erase the parent and children pointers diff --git a/gtsam/inference/BayesTree.cpp b/gtsam/inference/BayesTree.cpp index 6d6a4a4d3..2fb0227b6 100644 --- a/gtsam/inference/BayesTree.cpp +++ b/gtsam/inference/BayesTree.cpp @@ -20,7 +20,6 @@ #include -#include #include namespace gtsam { @@ -41,7 +40,7 @@ BayesTreeCliqueStats BayesTreeCliqueData::getStats() const double sum = 0.0; size_t max = 0; - BOOST_FOREACH(size_t s, conditionalSizes) { + for(size_t s: conditionalSizes) { sum += (double)s; if(s > max) max = s; } @@ -50,7 +49,7 @@ BayesTreeCliqueStats BayesTreeCliqueData::getStats() const sum = 0.0; max = 1; - BOOST_FOREACH(size_t s, separatorSizes) { + for(size_t s: separatorSizes) { sum += (double)s; if(s > max) max = s; } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 4d68acb5b..c22a5e257 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -30,7 +30,7 @@ namespace gtsam { // Forward declarations template class FactorGraph; - template class ClusterTree; + template class EliminatableClusterTree; /* ************************************************************************* */ /** clique statistics */ @@ -247,7 +247,7 @@ namespace gtsam { void fillNodesIndex(const sharedClique& subtree); // Friend JunctionTree because it directly fills roots and nodes index. - template friend class ClusterTree; + template friend class EliminatableClusterTree; private: /** Serialization function */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 256ff983d..31a8c55b6 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -18,7 +18,6 @@ #include #include -#include namespace gtsam { @@ -83,7 +82,7 @@ namespace gtsam { template size_t BayesTreeCliqueBase::treeSize() const { size_t size = 1; - BOOST_FOREACH(const derived_ptr& child, children) + for(const derived_ptr& child: children) size += child->treeSize(); return size; } @@ -96,7 +95,7 @@ namespace gtsam { return 0; size_t subtree_count = 1; - BOOST_FOREACH(const derived_ptr& child, children) + for(const derived_ptr& child: children) subtree_count += child->numCachedSeparatorMarginals(); return subtree_count; @@ -204,7 +203,7 @@ namespace gtsam { // root are also generated. So, if this clique's cached shortcut is set, // recursively call over all child cliques. Otherwise, it is unnecessary. if (cachedSeparatorMarginal_) { - BOOST_FOREACH(derived_ptr& child, children) { + for(derived_ptr& child: children) { child->deleteCachedShortcuts(); } diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 6b28f2dbf..b042c0c8e 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -1,5 +1,5 @@ /** - * @file ClusterTree-inst.h + * @file EliminatableClusterTree-inst.h * @date Oct 8, 2013 * @author Kai Ni * @author Richard Roberts @@ -7,17 +7,102 @@ * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree */ +#pragma once + #include #include #include #include #include -#include -#include - namespace gtsam { +/* ************************************************************************* */ +template +void ClusterTree::Cluster::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << " (" << problemSize_ << ")"; + PrintKeyVector(orderedFrontalKeys); +} + +/* ************************************************************************* */ +template +std::vector ClusterTree::Cluster::nrFrontalsOfChildren() const { + std::vector nrFrontals; + nrFrontals.reserve(nrChildren()); + for (const sharedNode& child : children) + nrFrontals.push_back(child->nrFrontals()); + return nrFrontals; +} + +/* ************************************************************************* */ +template +void ClusterTree::Cluster::merge(const boost::shared_ptr& cluster) { + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + orderedFrontalKeys.insert(orderedFrontalKeys.end(), cluster->orderedFrontalKeys.rbegin(), + cluster->orderedFrontalKeys.rend()); + factors.push_back(cluster->factors); + children.insert(children.end(), cluster->children.begin(), cluster->children.end()); + // Increment problem size + problemSize_ = std::max(problemSize_, cluster->problemSize_); +} + +/* ************************************************************************* */ +template +void ClusterTree::Cluster::mergeChildren( + const std::vector& merge) { + gttic(Cluster_mergeChildren); + assert(merge.size() == this->children.size()); + + // Count how many keys, factors and children we'll end up with + size_t nrKeys = orderedFrontalKeys.size(); + size_t nrFactors = factors.size(); + size_t nrNewChildren = 0; + // Loop over children + size_t i = 0; + for(const sharedNode& child: this->children) { + if (merge[i]) { + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrNewChildren += child->nrChildren(); + } else { + nrNewChildren += 1; // we keep the child + } + ++i; + } + + // now reserve space, and really merge + auto oldChildren = this->children; + this->children.clear(); + this->children.reserve(nrNewChildren); + orderedFrontalKeys.reserve(nrKeys); + factors.reserve(nrFactors); + i = 0; + for (const sharedNode& child : oldChildren) { + if (merge[i]) { + this->merge(child); + } else { + this->addChild(child); // we keep the child + } + ++i; + } + std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); +} + +/* ************************************************************************* */ +template +void ClusterTree::print(const std::string& s, const KeyFormatter& keyFormatter) const { + treeTraversal::PrintForest(*this, s, keyFormatter); +} + +/* ************************************************************************* */ +template +ClusterTree& ClusterTree::operator=(const This& other) { + // Start by duplicating the tree. + roots_ = treeTraversal::CloneForest(other); + return *this; +} + /* ************************************************************************* */ // Elimination traversal data - stores a pointer to the parent data and collects // the factors resulting from elimination of the children. Also sets up BayesTree @@ -35,6 +120,7 @@ struct EliminationData { size_t myIndexInParent; FastVector childFactors; boost::shared_ptr bayesTreeNode; + EliminationData(EliminationData* _parentData, size_t nChildren) : parentData(_parentData), bayesTreeNode(boost::make_shared()) { if (parentData) { @@ -56,7 +142,7 @@ struct EliminationData { const typename CLUSTERTREE::sharedNode& node, EliminationData& parentData) { assert(node); - EliminationData myData(&parentData, node->children.size()); + EliminationData myData(&parentData, node->nrChildren()); myData.bayesTreeNode->problemSize_ = node->problemSize(); return myData; } @@ -76,120 +162,51 @@ struct EliminationData { } // Function that does the HEAVY lifting - void operator()(const typename CLUSTERTREE::sharedNode& node, - EliminationData& myData) { + void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) { assert(node); // Gather factors FactorGraphType gatheredFactors; - gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors.reserve(node->factors.size() + node->nrChildren()); gatheredFactors += node->factors; gatheredFactors += myData.childFactors; // Check for Bayes tree orphan subtrees, and add them to our children - BOOST_FOREACH(const sharedFactor& f, node->factors) { - if (const BayesTreeOrphanWrapper* asSubtree = - dynamic_cast*>(f.get())) { + // TODO(frank): should this really happen here? + for (const sharedFactor& factor: node->factors) { + auto asSubtree = dynamic_cast*>(factor.get()); + if (asSubtree) { myData.bayesTreeNode->children.push_back(asSubtree->clique); asSubtree->clique->parent_ = myData.bayesTreeNode; } } // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>> - std::pair, - boost::shared_ptr > eliminationResult = - eliminationFunction_(gatheredFactors, node->orderedFrontalKeys); + auto eliminationResult = eliminationFunction_(gatheredFactors, node->orderedFrontalKeys); // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< - // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the + // remaining factor myData.bayesTreeNode->setEliminationResult(eliminationResult); // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 // object they're added to. - BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) + for (const Key& j: myData.bayesTreeNode->conditional()->frontals()) nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode)); // Store remaining factor in parent's gathered factors if (!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = - eliminationResult.second; + myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; } }; }; /* ************************************************************************* */ template -void ClusterTree::Cluster::print(const std::string& s, - const KeyFormatter& keyFormatter) const { - std::cout << s << " (" << problemSize_ << ")"; - PrintKeyVector(orderedFrontalKeys); -} - -/* ************************************************************************* */ -template -void ClusterTree::Cluster::mergeChildren( - const std::vector& merge) { - gttic(Cluster_mergeChildren); - - // Count how many keys, factors and children we'll end up with - size_t nrKeys = orderedFrontalKeys.size(); - size_t nrFactors = factors.size(); - size_t nrNewChildren = 0; - // Loop over children - size_t i = 0; - BOOST_FOREACH(const sharedNode& child, children) { - if (merge[i]) { - nrKeys += child->orderedFrontalKeys.size(); - nrFactors += child->factors.size(); - nrNewChildren += child->children.size(); - } else { - nrNewChildren += 1; // we keep the child - } - ++i; - } - - // now reserve space, and really merge - orderedFrontalKeys.reserve(nrKeys); - factors.reserve(nrFactors); - typename Node::Children newChildren; - newChildren.reserve(nrNewChildren); - i = 0; - BOOST_FOREACH(const sharedNode& child, children) { - if (merge[i]) { - // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. - orderedFrontalKeys.insert(orderedFrontalKeys.end(), - child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); - // Merge keys, factors, and children. - factors.insert(factors.end(), child->factors.begin(), - child->factors.end()); - newChildren.insert(newChildren.end(), child->children.begin(), - child->children.end()); - // Increment problem size - problemSize_ = std::max(problemSize_, child->problemSize_); - // Increment number of frontal variables - } else { - newChildren.push_back(child); // we keep the child - } - ++i; - } - children = newChildren; - std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); -} - -/* ************************************************************************* */ -template -void ClusterTree::print(const std::string& s, - const KeyFormatter& keyFormatter) const { - treeTraversal::PrintForest(*this, s, keyFormatter); -} - -/* ************************************************************************* */ -template -ClusterTree& ClusterTree::operator=( +EliminatableClusterTree& EliminatableClusterTree::operator=( const This& other) { - // Start by duplicating the tree. - roots_ = treeTraversal::CloneForest(other); + ClusterTree::operator=(other); // Assign the remaining factors - these are pointers to factors in the original factor graph and // we do not clone them. @@ -199,41 +216,40 @@ ClusterTree& ClusterTree::operator=( } /* ************************************************************************* */ -template -std::pair, boost::shared_ptr > ClusterTree< - BAYESTREE, GRAPH>::eliminate(const Eliminate& function) const { +template +std::pair, boost::shared_ptr > +EliminatableClusterTree::eliminate(const Eliminate& function) const { gttic(ClusterTree_eliminate); // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node // that contains all of the roots as its children. rootsContainer also stores the remaining - // uneliminated factors passed up from the roots. + // un-eliminated factors passed up from the roots. boost::shared_ptr result = boost::make_shared(); + typedef EliminationData Data; - Data rootsContainer(0, roots_.size()); - typename Data::EliminationPostOrderVisitor visitorPost(function, - result->nodes_); + Data rootsContainer(0, this->nrRoots()); + + typename Data::EliminationPostOrderVisitor visitorPost(function, result->nodes_); { - TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP - treeTraversal::DepthFirstForestParallel(*this, rootsContainer, - Data::EliminationPreOrderVisitor, visitorPost, 10); + TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + treeTraversal::DepthFirstForestParallel(*this, rootsContainer, Data::EliminationPreOrderVisitor, + visitorPost, 10); } // Create BayesTree from roots stored in the dummy BayesTree node. - result->roots_.insert(result->roots_.end(), - rootsContainer.bayesTreeNode->children.begin(), - rootsContainer.bayesTreeNode->children.end()); + result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), + rootsContainer.bayesTreeNode->children.end()); // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr remaining = boost::make_shared< - FactorGraphType>(); - remaining->reserve( - remainingFactors_.size() + rootsContainer.childFactors.size()); + boost::shared_ptr remaining = boost::make_shared(); + remaining->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); remaining->push_back(remainingFactors_.begin(), remainingFactors_.end()); - BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) { + for (const sharedFactor& factor : rootsContainer.childFactors) { if (factor) remaining->push_back(factor); } + // Return result return std::make_pair(result, remaining); } -} +} // namespace gtsam diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index b00532d22..e225bac5f 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -1,5 +1,5 @@ /** - * @file ClusterTree.h + * @file EliminatableClusterTree.h * @date Oct 8, 2013 * @author Kai Ni * @author Richard Roberts @@ -21,57 +21,182 @@ namespace gtsam { * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. * \nosubgrouping */ -template +template class ClusterTree { -public: - typedef GRAPH FactorGraphType; ///< The factor graph type - typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef ClusterTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination - typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals - typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine + public: + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef ClusterTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + + /// A Cluster is just a collection of factors + // TODO(frank): re-factor JunctionTree so we can make members private struct Cluster { - typedef Ordering Keys; - typedef FastVector Factors; typedef FastVector > Children; + Children children; ///< sub-trees - Cluster() { - } - Cluster(Key key, const Factors& factors) : - factors(factors) { - orderedFrontalKeys.push_back(key); - } + typedef Ordering Keys; + Keys orderedFrontalKeys; ///< Frontal keys of this node + + FactorGraphType factors; ///< Factors associated with this node - Keys orderedFrontalKeys; ///< Frontal keys of this node - Factors factors; ///< Factors associated with this node - Children children; ///< sub-trees int problemSize_; + Cluster() : problemSize_(0) {} + + virtual ~Cluster() {} + + const Cluster& operator[](size_t i) const { + return *(children[i]); + } + + /// Construct from factors associated with a single key + template + Cluster(Key key, const CONTAINER& factorsToAdd) + : problemSize_(0) { + addFactors(key, factorsToAdd); + } + + /// Add factors associated with a single key + template + void addFactors(Key key, const CONTAINER& factorsToAdd) { + orderedFrontalKeys.push_back(key); + factors.push_back(factorsToAdd); + problemSize_ += factors.size(); + } + + /// Add a child cluster + void addChild(const boost::shared_ptr& cluster) { + children.push_back(cluster); + problemSize_ = std::max(problemSize_, cluster->problemSize_); + } + + size_t nrChildren() const { + return children.size(); + } + + size_t nrFactors() const { + return factors.size(); + } + + size_t nrFrontals() const { + return orderedFrontalKeys.size(); + } + int problemSize() const { return problemSize_; } /// print this node - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// Return a vector with nrFrontal keys for each child + std::vector nrFrontalsOfChildren() const; + + /// Merge in given cluster + void merge(const boost::shared_ptr& cluster); /// Merge all children for which bit is set into this node void mergeChildren(const std::vector& merge); }; - typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster - typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions - typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions + typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster + + // Define Node=Cluster for compatibility with tree traversal functions + typedef Cluster Node; + typedef sharedCluster sharedNode; /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); -protected: + protected: FastVector roots_; + + /// @name Standard Constructors + /// @{ + + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + ClusterTree(const This& other) { + *this = other; + } + + /// @} + + public: + + /// Default constructor + ClusterTree() {} + + /// @name Testable + /// @{ + + /** Print the cluster tree */ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + + /// @name Advanced Interface + /// @{ + + void addRoot(const boost::shared_ptr& cluster) { + roots_.push_back(cluster); + } + + void addChildrenAsRoots(const boost::shared_ptr& cluster) { + for (auto child : cluster->children) + this->addRoot(child); + } + + size_t nrRoots() const { + return roots_.size(); + } + + /** Return the set of roots (one for a tree, multiple for a forest) */ + const FastVector& roots() const { + return roots_; + } + + const Cluster& operator[](size_t i) const { + return *(roots_[i]); + } + + /// @} + + protected: + /// @name Details + + /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors + /// are copied, factors are not cloned. + This& operator=(const This& other); + + /// @} +}; + +/** + * A cluster-tree that eliminates to a Bayes tree. + */ +template +class EliminatableClusterTree : public ClusterTree { + public: + typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef EliminatableClusterTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + typedef typename BAYESTREE::ConditionalType ConditionalType; ///< The type of conditionals + typedef boost::shared_ptr + sharedConditional; ///< Shared pointer to a conditional + + typedef typename GRAPH::Eliminate Eliminate; ///< Typedef for an eliminate subroutine + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + + protected: FastVector remainingFactors_; /// @name Standard Constructors @@ -79,19 +204,13 @@ protected: /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ - ClusterTree(const This& other) {*this = other;} - - /// @} - -public: - /// @name Testable - /// @{ - - /** Print the cluster tree */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + EliminatableClusterTree(const This& other) : ClusterTree(other) { + *this = other; + } /// @} + public: /// @name Standard Interface /// @{ @@ -100,23 +219,22 @@ public: * in GaussianFactorGraph.h * @return The Bayes tree and factor graph resulting from elimination */ - std::pair, boost::shared_ptr > - eliminate(const Eliminate& function) const; + std::pair, boost::shared_ptr > eliminate( + const Eliminate& function) const; /// @} /// @name Advanced Interface /// @{ - /** Return the set of roots (one for a tree, multiple for a forest) */ - const FastVector& roots() const {return roots_;} - /** Return the remaining factors that are not pulled into elimination */ - const FastVector& remainingFactors() const {return remainingFactors_;} + const FastVector& remainingFactors() const { + return remainingFactors_; + } /// @} -protected: + protected: /// @name Details /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors @@ -124,11 +242,10 @@ protected: This& operator=(const This& other); /// Default constructor to be used in derived classes - ClusterTree() {} + EliminatableClusterTree() {} /// @} - }; - } +#include diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h index a22f290d9..39b60079c 100644 --- a/gtsam/inference/Conditional-inst.h +++ b/gtsam/inference/Conditional-inst.h @@ -18,7 +18,6 @@ // \callgraph #pragma once -#include #include #include @@ -29,11 +28,11 @@ namespace gtsam { template void Conditional::print(const std::string& s, const KeyFormatter& formatter) const { std::cout << s << " P("; - BOOST_FOREACH(Key key, frontals()) + for(Key key: frontals()) std::cout << " " << formatter(key); if (nrParents() > 0) std::cout << " |"; - BOOST_FOREACH(Key parent, parents()) + for(Key parent: parents()) std::cout << " " << formatter(parent); std::cout << ")" << std::endl; } diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 68e623b68..90540aaa6 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -17,7 +17,6 @@ */ #pragma once -#include #include #include #include @@ -66,7 +65,7 @@ namespace gtsam { const std::string& str, const KeyFormatter& keyFormatter) const { std::cout << str << "(" << keyFormatter(key) << ")\n"; - BOOST_FOREACH(const sharedFactor& factor, factors) { + for(const sharedFactor& factor: factors) { if(factor) factor->print(str); else @@ -107,7 +106,7 @@ namespace gtsam { // for row i \in Struct[A*j] do node->children.reserve(factors.size()); node->factors.reserve(factors.size()); - BOOST_FOREACH(const size_t i, factors) { + for(const size_t i: factors) { // If we already hit a variable in this factor, make the subtree containing the previous // variable in this factor a child of the current node. This means that the variables // eliminated earlier in the factor depend on the later variables in the factor. If we @@ -222,15 +221,15 @@ namespace gtsam { // Add roots in sorted order { FastMap keys; - BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(std::make_pair(root->key, root)); } + for(const sharedNode& root: this->roots_) { keys.insert(std::make_pair(root->key, root)); } typedef typename FastMap::value_type Key_Node; - BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); } + for(const Key_Node& key_node: keys) { stack1.push(key_node.second); } } { FastMap keys; - BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(std::make_pair(root->key, root)); } + for(const sharedNode& root: expected.roots_) { keys.insert(std::make_pair(root->key, root)); } typedef typename FastMap::value_type Key_Node; - BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); } + for(const Key_Node& key_node: keys) { stack2.push(key_node.second); } } // Traverse, adding children in sorted order @@ -262,15 +261,15 @@ namespace gtsam { // Add children in sorted order { FastMap keys; - BOOST_FOREACH(const sharedNode& node, node1->children) { keys.insert(std::make_pair(node->key, node)); } + for(const sharedNode& node: node1->children) { keys.insert(std::make_pair(node->key, node)); } typedef typename FastMap::value_type Key_Node; - BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); } + for(const Key_Node& key_node: keys) { stack1.push(key_node.second); } } { FastMap keys; - BOOST_FOREACH(const sharedNode& node, node2->children) { keys.insert(std::make_pair(node->key, node)); } + for(const sharedNode& node: node2->children) { keys.insert(std::make_pair(node->key, node)); } typedef typename FastMap::value_type Key_Node; - BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); } + for(const Key_Node& key_node: keys) { stack2.push(key_node.second); } } } diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp index 707193a52..49f34fb2b 100644 --- a/gtsam/inference/Factor.cpp +++ b/gtsam/inference/Factor.cpp @@ -19,7 +19,6 @@ // \callgraph -#include #include #include @@ -35,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const { std::cout << s << " "; - BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key); + for(Key key: keys_) std::cout << " " << formatter(key); std::cout << std::endl; } @@ -44,4 +43,4 @@ namespace gtsam { return keys_ == other.keys_; } -} \ No newline at end of file +} diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 52ba23155..0aa4af2e1 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -23,7 +23,6 @@ #include -#include #include #include @@ -66,7 +65,7 @@ namespace gtsam { template size_t FactorGraph::nrFactors() const { size_t size_ = 0; - BOOST_FOREACH(const sharedFactor& factor, factors_) + for(const sharedFactor& factor: factors_) if (factor) size_++; return size_; } @@ -75,7 +74,7 @@ namespace gtsam { template KeySet FactorGraph::keys() const { KeySet keys; - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + for(const sharedFactor& factor: this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); } @@ -87,7 +86,7 @@ namespace gtsam { KeyVector FactorGraph::keyVector() const { KeyVector keys; keys.reserve(2 * size()); // guess at size - BOOST_FOREACH (const sharedFactor& factor, factors_) + for (const sharedFactor& factor: factors_) if (factor) keys.insert(keys.end(), factor->begin(), factor->end()); std::sort(keys.begin(), keys.end()); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index fcafc9b42..016488701 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -22,16 +22,17 @@ #pragma once +#include +#include +#include + #include #include #include #include -#include -#include -#include -#include -#include +#include +#include namespace gtsam { @@ -151,7 +152,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(boost::shared_ptr factor) { factors_.push_back(boost::shared_ptr(factor)); } @@ -159,15 +160,22 @@ namespace gtsam { void push_back(const sharedFactor& factor) { factors_.push_back(factor); } + /** Emplace a factor */ + template + typename std::enable_if::value>::type + emplace_shared(Args&&... args) { + factors_.push_back(boost::make_shared(std::forward(args)...)); + } + /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { factors_.insert(end(), firstFactor, lastFactor); } /** push back many factors as shared_ptr's in a container (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } @@ -175,22 +183,24 @@ namespace gtsam { /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived * classes in favor of a type-specialized version that calls this templated function. */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const BayesTree& bayesTree) { bayesTree.addFactorsToGraph(*this); } +//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid * the copy). */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const DERIVEDFACTOR& factor) { factors_.push_back(boost::make_shared(factor)); } +//#endif /** push back many factors with an iterator over plain factors (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f); @@ -198,14 +208,14 @@ namespace gtsam { /** push back many factors as non-pointer objects in a container (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if, + typename std::enable_if::value, boost::assign::list_inserter > >::type operator+=(boost::shared_ptr factor) { return boost::assign::make_list_inserter(RefCallPushBack(*this))(factor); @@ -226,7 +236,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type add(boost::shared_ptr factor) { push_back(factor); } @@ -354,3 +364,5 @@ namespace gtsam { }; // FactorGraph } // namespace gtsam + +#include diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 99f668591..bf2cacc84 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -39,7 +39,7 @@ namespace gtsam { factors += newFactors; // Add the orphaned subtrees - BOOST_FOREACH(const sharedClique& orphan, orphans) + for(const sharedClique& orphan: orphans) factors += boost::make_shared >(orphan); // eliminate into a Bayes net diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 352a8dded..04472f7e3 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -54,7 +54,7 @@ struct ConstructorTraversalData { // pointer in its parent. ConstructorTraversalData myData = ConstructorTraversalData(&parentData); myData.myJTNode = boost::make_shared(node->key, node->factors); - parentData.myJTNode->children.push_back(myData.myJTNode); + parentData.myJTNode->addChild(myData.myJTNode); return myData; } @@ -99,20 +99,20 @@ struct ConstructorTraversalData { // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. const size_t myNrParents = myConditional->nrParents(); - const size_t nrChildren = node->children.size(); + const size_t nrChildren = node->nrChildren(); assert(childConditionals.size() == nrChildren); // decide which children to merge, as index into children + std::vector nrFrontals = node->nrFrontalsOfChildren(); std::vector merge(nrChildren, false); - size_t myNrFrontals = 1, i = 0; - BOOST_FOREACH(const sharedNode& child, node->children) { + size_t myNrFrontals = 1; + for (size_t i = 0;inrParents()) { // Increment number of frontal variables - myNrFrontals += child->orderedFrontalKeys.size(); + myNrFrontals += nrFrontals[i]; merge[i] = true; } - ++i; } // now really merge @@ -145,10 +145,7 @@ JunctionTree::JunctionTree( Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - typedef typename JunctionTree::Node Node; - const typename Node::Children& children = rootData.myJTNode->children; - Base::roots_.reserve(children.size()); - Base::roots_.insert(Base::roots_.begin(), children.begin(), children.end()); + this->addChildrenAsRoots(rootData.myJTNode); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors(); diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index cdf38b97c..e01f3721a 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -28,15 +28,13 @@ namespace gtsam { template class EliminationTree; /** - * A JunctionTree is a ClusterTree, i.e., a set of variable clusters with factors, arranged - * in a tree, with the additional property that it represents the clique tree associated - * with a Bayes net. + * A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree, + * with the additional property that it represents the clique tree associated with a Bayes Net. * - * In GTSAM a junction tree is an intermediate data structure in multifrontal - * variable elimination. Each node is a cluster of factors, along with a - * clique of variables that are eliminated all at once. In detail, every node k represents - * a clique (maximal fully connected subset) of an associated chordal graph, such as a - * chordal Bayes net resulting from elimination. + * In GTSAM a junction tree is an intermediate data structure in multifrontal variable + * elimination. Each node is a cluster of factors, along with a clique of variables that are + * eliminated all at once. In detail, every node k represents a clique (maximal fully connected + * subset) of an associated chordal graph, such as a chordal Bayes net resulting from elimination. * * The difference with the BayesTree is that a JunctionTree stores factors, whereas a * BayesTree stores conditionals, that are the product of eliminating the factors in the @@ -49,13 +47,13 @@ namespace gtsam { * \nosubgrouping */ template - class JunctionTree : public ClusterTree { + class JunctionTree : public EliminatableClusterTree { public: typedef JunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef ClusterTree Base; ///< Our base class + typedef EliminatableClusterTree Base; ///< Our base class protected: @@ -65,7 +63,7 @@ namespace gtsam { /** Build the junction tree from an elimination tree. */ template static This FromEliminationTree(const ETREE& eliminationTree) { return This(eliminationTree); } - + /** Build the junction tree from an elimination tree. */ template JunctionTree(const EliminationTree& eliminationTree); diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index a2a6906d1..f25727441 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -20,7 +20,6 @@ #include #include -#include #include #include @@ -63,7 +62,7 @@ static void Print(const CONTAINER& keys, const string& s, if (keys.empty()) cout << "(none)" << endl; else { - BOOST_FOREACH(const Key& key, keys) + for(const Key& key: keys) cout << keyFormatter(key) << " "; cout << endl; } diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 06e5ddeec..eb9670254 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -17,7 +17,6 @@ #pragma once -#include #include #include @@ -42,7 +41,7 @@ void MetisIndex::augment(const FactorGraph& factors) { // key to integer mapping. This is referenced during the adjaceny step for (size_t i = 0; i < factors.size(); i++) { if (factors[i]) { - BOOST_FOREACH(const Key& key, *factors[i]) { + for(const Key& key: *factors[i]) { keySet.insert(keySet.end(), key); // Keep a track of all unique keys if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) { intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); @@ -55,8 +54,8 @@ void MetisIndex::augment(const FactorGraph& factors) { // Create an adjacency mapping that stores the set of all adjacent keys for every key for (size_t i = 0; i < factors.size(); i++) { if (factors[i]) { - BOOST_FOREACH(const Key& k1, *factors[i]) - BOOST_FOREACH(const Key& k2, *factors[i]) + for(const Key& k1: *factors[i]) + for(const Key& k2: *factors[i]) if (k1 != k2) { // Store both in Key and int32_t format int i = intKeyBMap_.left.at(k1); diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 3e3b40f8f..e12c32df3 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -53,10 +53,10 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, gttic(Ordering_COLAMDConstrained); gttic(Prepare); - size_t nEntries = variableIndex.nEntries(), nFactors = + const size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); // Convert to compressed column major format colamd wants it in (== MATLAB format!) - size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, + const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, (int) nVars); /* colamd arg 3: size of the array A */ vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ @@ -66,16 +66,13 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, int count = 0; vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order size_t index = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { + for (auto key_factors: variableIndex) { // Arrange factor indices into COLAMD format const VariableIndex::Factors& column = key_factors.second; - size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(size_t factorIndex, column) { - if (lastFactorId != numeric_limits::max()) - assert(factorIndex > lastFactorId); + for(size_t factorIndex: column) { A[count++] = (int) factorIndex; // copy sparse column } - p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 + p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 // Store key in array and increment index keys[index] = key_factors.first; ++index; @@ -106,8 +103,8 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, // ccolamd_report(stats); - gttic(Fill_Ordering); // Convert elimination ordering in p to an ordering + gttic(Fill_Ordering); Ordering result; result.resize(nVars); for (size_t j = 0; j < nVars; ++j) @@ -126,15 +123,16 @@ Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex, std::vector cmember(n, 0); // Build a mapping to look up sorted Key indices by Key + // TODO(frank): think of a way to not build this FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // If at least some variables are not constrained to be last, constrain the // ones that should be constrained. int group = (constrainLast.size() != n ? 1 : 0); - BOOST_FOREACH(Key key, constrainLast) { + for (Key key: constrainLast) { cmember[keyIndices.at(key)] = group; if (forceOrder) ++group; @@ -155,13 +153,13 @@ Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, // Build a mapping to look up sorted Key indices by Key FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // If at least some variables are not constrained to be last, constrain the // ones that should be constrained. int group = 0; - BOOST_FOREACH(Key key, constrainFirst) { + for (Key key: constrainFirst) { cmember[keyIndices.at(key)] = group; if (forceOrder) ++group; @@ -169,7 +167,7 @@ Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, if (!forceOrder && !constrainFirst.empty()) ++group; - BOOST_FOREACH(int& c, cmember) + for(int& c: cmember) if (c == none) c = group; @@ -186,12 +184,12 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, // Build a mapping to look up sorted Key indices by Key FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // Assign groups typedef FastMap::value_type key_group; - BOOST_FOREACH(const key_group& p, groups) { + for(const key_group& p: groups) { // FIXME: check that no groups are skipped cmember[keyIndices.at(p.first)] = p.second; } diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 8af2649c5..ae7a10f44 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -79,7 +79,10 @@ public: /// it is faster to use COLAMD(const VariableIndex&) template static Ordering Colamd(const FactorGraph& graph) { - return Colamd(VariableIndex(graph)); + if (graph.empty()) + return Ordering(); + else + return Colamd(VariableIndex(graph)); } /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. @@ -96,8 +99,10 @@ public: template static Ordering ColamdConstrainedLast(const FactorGraph& graph, const std::vector& constrainLast, bool forceOrder = false) { - return ColamdConstrainedLast(VariableIndex(graph), constrainLast, - forceOrder); + if (graph.empty()) + return Ordering(); + else + return ColamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This @@ -121,8 +126,10 @@ public: template static Ordering ColamdConstrainedFirst(const FactorGraph& graph, const std::vector& constrainFirst, bool forceOrder = false) { - return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, - forceOrder); + if (graph.empty()) + return Ordering(); + else + return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This @@ -148,7 +155,10 @@ public: template static Ordering ColamdConstrained(const FactorGraph& graph, const FastMap& groups) { - return ColamdConstrained(VariableIndex(graph), groups); + if (graph.empty()) + return Ordering(); + else + return ColamdConstrained(VariableIndex(graph), groups); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this @@ -190,6 +200,8 @@ public: template static Ordering Create(OrderingType orderingType, const FactorGraph& graph) { + if (graph.empty()) + return Ordering(); switch (orderingType) { case COLAMD: diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 102761273..2d7ede8e7 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -33,8 +33,8 @@ namespace gtsam { */ class GTSAM_EXPORT Symbol { protected: - const unsigned char c_; - const std::uint64_t j_; + unsigned char c_; + std::uint64_t j_; public: diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index c00a3633e..352ea166f 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -19,7 +19,6 @@ #include #include -#include namespace gtsam { @@ -34,7 +33,7 @@ void VariableIndex::augment(const FG& factors, if (factors[i]) { const size_t globalI = newFactorIndices ? (*newFactorIndices)[i] : nFactors_; - BOOST_FOREACH(const Key key, *factors[i]) { + for(const Key key: *factors[i]) { index_[key].push_back(globalI); ++nEntries_; } @@ -67,7 +66,7 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, throw std::invalid_argument( "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); if (factors[i]) { - BOOST_FOREACH(Key j, *factors[i]) { + for(Key j: *factors[i]) { Factors& factorEntries = internalAt(j); Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index e67c0e248..72c56be5b 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -33,9 +33,9 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const { void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str; cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; - BOOST_FOREACH(KeyMap::value_type key_factors, index_) { + for(KeyMap::value_type key_factors: index_) { cout << "var " << keyFormatter(key_factors.first) << ":"; - BOOST_FOREACH(const size_t factor, key_factors.second) + for(const size_t factor: key_factors.second) cout << " " << factor; cout << "\n"; } @@ -46,9 +46,9 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c void VariableIndex::outputMetisFormat(ostream& os) const { os << size() << " " << nFactors() << "\n"; // run over variables, which will be hyper-edges. - BOOST_FOREACH(KeyMap::value_type key_factors, index_) { + for(KeyMap::value_type key_factors: index_) { // every variable is a hyper-edge covering its factors - BOOST_FOREACH(const size_t factor, key_factors.second) + for(const size_t factor: key_factors.second) os << (factor+1) << " "; // base 1 os << "\n"; } diff --git a/gtsam/inference/VariableSlots.cpp b/gtsam/inference/VariableSlots.cpp index 783071ae2..d89ad2ced 100644 --- a/gtsam/inference/VariableSlots.cpp +++ b/gtsam/inference/VariableSlots.cpp @@ -18,7 +18,6 @@ #include #include -#include using namespace std; @@ -33,12 +32,12 @@ void VariableSlots::print(const std::string& str) const { else { cout << str << "\n"; cout << "Var:\t"; - BOOST_FOREACH(const value_type& slot, *this) { cout << slot.first << "\t"; } + for(const value_type& slot: *this) { cout << slot.first << "\t"; } cout << "\n"; for(size_t i=0; ibegin()->second.size(); ++i) { cout << " \t"; - BOOST_FOREACH(const value_type& slot, *this) { + for(const value_type& slot: *this) { if(slot.second[i] == Empty) cout << "x" << "\t"; else diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index fad789436..8ebd5c13c 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -24,7 +24,6 @@ #include #include -#include #include #include @@ -99,10 +98,10 @@ VariableSlots::VariableSlots(const FG& factorGraph) // removed factors. The slot number is the max integer value if the // factor does not involve that variable. size_t jointFactorPos = 0; - BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) { + for(const typename FG::sharedFactor& factor: factorGraph) { assert(factor); size_t factorVarSlot = 0; - BOOST_FOREACH(const Key involvedVariable, *factor) { + for(const Key involvedVariable: *factor) { // Set the slot in this factor for this variable. If the // variable was not already discovered, create an array for it // that we'll fill with the slot indices for each factor that diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 90c7b32c9..de614c2b4 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -18,7 +18,6 @@ #pragma once #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -32,8 +31,6 @@ #include -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - namespace gtsam { /* ************************************************************************* */ @@ -123,9 +120,10 @@ predecessorMap2Graph(const PredecessorMap& p_map) { G g; std::map key2vertex; V v1, v2, root; - KEY child, parent; bool foundRoot = false; - FOREACH_PAIR(child, parent, p_map) { + for(auto child_parent: p_map) { + KEY child, parent; + std::tie(child,parent) = child_parent; if (key2vertex.find(child) == key2vertex.end()) { v1 = add_vertex(child, g); key2vertex.insert(std::make_pair(child, v1)); @@ -193,7 +191,7 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap // attach the relative poses to the edges PoseEdge edge12, edge21; bool found1, found2; - BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) { + for(typename G::sharedFactor nl_factor: graph) { if (nl_factor->keys().size() > 2) throw std::invalid_argument("composePoses: only support factors with at most two keys"); @@ -243,7 +241,7 @@ PredecessorMap findMinimumSpanningTree(const G& fg) { // convert edge to string pairs PredecessorMap tree; typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; - BOOST_FOREACH(const typename SDGraph::Vertex& vi, p_map){ + for(const typename SDGraph::Vertex& vi: p_map){ KEY key = boost::get(boost::vertex_name, g, *itVertex); KEY parent = boost::get(boost::vertex_name, g, vi); tree.insert(key, parent); @@ -258,7 +256,7 @@ void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { typedef typename G::sharedFactor F ; - BOOST_FOREACH(const F& factor, g) + for(const F& factor: g) { if (factor->keys().size() > 2) throw(std::invalid_argument("split: only support factors with at most two keys")); diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index c209d086b..0d23d9655 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -30,36 +30,64 @@ using namespace boost::assign; namespace example { SymbolicFactorGraph symbolicChain() { - SymbolicFactorGraph sfg; - sfg.push_factor(0, 1); - sfg.push_factor(1, 2); - sfg.push_factor(2, 3); - sfg.push_factor(3, 4); - sfg.push_factor(4, 5); - return sfg; + SymbolicFactorGraph symbolicGraph; + symbolicGraph.push_factor(0, 1); + symbolicGraph.push_factor(1, 2); + symbolicGraph.push_factor(2, 3); + symbolicGraph.push_factor(3, 4); + symbolicGraph.push_factor(4, 5); + return symbolicGraph; } } /* ************************************************************************* */ TEST(Ordering, constrained_ordering) { - // create graph with wanted variable set = 2, 4 - SymbolicFactorGraph sfg = example::symbolicChain(); + SymbolicFactorGraph symbolicGraph = example::symbolicChain(); // unconstrained version - Ordering actUnconstrained = Ordering::Colamd(sfg); - Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5)); - EXPECT(assert_equal(expUnconstrained, actUnconstrained)); + { + Ordering actual = Ordering::Colamd(symbolicGraph); + Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + EXPECT(assert_equal(expected, actual)); + } // constrained version - push one set to the end - Ordering actConstrained = Ordering::ColamdConstrainedLast(sfg, list_of(2)(4)); - Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2)); - EXPECT(assert_equal(expConstrained, actConstrained)); + { + Ordering actual = Ordering::ColamdConstrainedLast(symbolicGraph, list_of(2)(4)); + Ordering expected = Ordering(list_of(0)(1)(5)(3)(4)(2)); + EXPECT(assert_equal(expected, actual)); + } // constrained version - push one set to the start - Ordering actConstrained2 = Ordering::ColamdConstrainedFirst(sfg, - list_of(2)(4)); - Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); - EXPECT(assert_equal(expConstrained2, actConstrained2)); + { + Ordering actual = Ordering::ColamdConstrainedFirst(symbolicGraph, list_of(2)(4)); + Ordering expected = Ordering(list_of(2)(4)(0)(1)(3)(5)); + EXPECT(assert_equal(expected, actual)); + } + + // Make sure giving empty constraints does not break the code + { + Ordering actual = Ordering::ColamdConstrainedLast(symbolicGraph, {}); + Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + EXPECT(assert_equal(expected, actual)); + } + { + Ordering actual = Ordering::ColamdConstrainedFirst(symbolicGraph, {}); + Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + EXPECT(assert_equal(expected, actual)); + } + + // Make sure giving empty graph does not break the code + SymbolicFactorGraph emptyGraph; + Ordering empty; + { + Ordering actual = Ordering::ColamdConstrainedLast(emptyGraph, list_of(2)(4)); + EXPECT(assert_equal(empty, actual)); + } + { + Ordering actual = Ordering::ColamdConstrainedFirst(emptyGraph, list_of(2)(4)); + EXPECT(assert_equal(empty, actual)); + } } /* ************************************************************************* */ @@ -68,7 +96,7 @@ TEST(Ordering, grouped_constrained_ordering) { // create graph with constrained groups: // 1: 2, 4 // 2: 5 - SymbolicFactorGraph sfg = example::symbolicChain(); + SymbolicFactorGraph symbolicGraph = example::symbolicChain(); // constrained version - push one set to the end FastMap constraints; @@ -76,40 +104,40 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[4] = 1; constraints[5] = 2; - Ordering actConstrained = Ordering::ColamdConstrained(sfg, constraints); - Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5); - EXPECT(assert_equal(expConstrained, actConstrained)); + Ordering actual = Ordering::ColamdConstrained(symbolicGraph, constraints); + Ordering expected = list_of(0)(1)(3)(2)(4)(5); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(Ordering, csr_format) { // Example in METIS manual - SymbolicFactorGraph sfg; - sfg.push_factor(0, 1); - sfg.push_factor(1, 2); - sfg.push_factor(2, 3); - sfg.push_factor(3, 4); - sfg.push_factor(5, 6); - sfg.push_factor(6, 7); - sfg.push_factor(7, 8); - sfg.push_factor(8, 9); - sfg.push_factor(10, 11); - sfg.push_factor(11, 12); - sfg.push_factor(12, 13); - sfg.push_factor(13, 14); + SymbolicFactorGraph symbolicGraph; + symbolicGraph.push_factor(0, 1); + symbolicGraph.push_factor(1, 2); + symbolicGraph.push_factor(2, 3); + symbolicGraph.push_factor(3, 4); + symbolicGraph.push_factor(5, 6); + symbolicGraph.push_factor(6, 7); + symbolicGraph.push_factor(7, 8); + symbolicGraph.push_factor(8, 9); + symbolicGraph.push_factor(10, 11); + symbolicGraph.push_factor(11, 12); + symbolicGraph.push_factor(12, 13); + symbolicGraph.push_factor(13, 14); - sfg.push_factor(0, 5); - sfg.push_factor(5, 10); - sfg.push_factor(1, 6); - sfg.push_factor(6, 11); - sfg.push_factor(2, 7); - sfg.push_factor(7, 12); - sfg.push_factor(3, 8); - sfg.push_factor(8, 13); - sfg.push_factor(4, 9); - sfg.push_factor(9, 14); + symbolicGraph.push_factor(0, 5); + symbolicGraph.push_factor(5, 10); + symbolicGraph.push_factor(1, 6); + symbolicGraph.push_factor(6, 11); + symbolicGraph.push_factor(2, 7); + symbolicGraph.push_factor(7, 12); + symbolicGraph.push_factor(3, 8); + symbolicGraph.push_factor(8, 13); + symbolicGraph.push_factor(4, 9); + symbolicGraph.push_factor(9, 14); - MetisIndex mi(sfg); + MetisIndex mi(symbolicGraph); vector xadjExpected, adjExpected; xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; @@ -122,16 +150,16 @@ TEST(Ordering, csr_format) { /* ************************************************************************* */ TEST(Ordering, csr_format_2) { - SymbolicFactorGraph sfg; + SymbolicFactorGraph symbolicGraph; - sfg.push_factor(0); - sfg.push_factor(0, 1); - sfg.push_factor(1, 2); - sfg.push_factor(2, 3); - sfg.push_factor(3, 4); - sfg.push_factor(4, 1); + symbolicGraph.push_factor(0); + symbolicGraph.push_factor(0, 1); + symbolicGraph.push_factor(1, 2); + symbolicGraph.push_factor(2, 3); + symbolicGraph.push_factor(3, 4); + symbolicGraph.push_factor(4, 1); - MetisIndex mi(sfg); + MetisIndex mi(symbolicGraph); vector xadjExpected, adjExpected; xadjExpected += 0, 1, 4, 6, 8, 10; @@ -144,16 +172,16 @@ TEST(Ordering, csr_format_2) { /* ************************************************************************* */ TEST(Ordering, csr_format_3) { - SymbolicFactorGraph sfg; + SymbolicFactorGraph symbolicGraph; - sfg.push_factor(100); - sfg.push_factor(100, 101); - sfg.push_factor(101, 102); - sfg.push_factor(102, 103); - sfg.push_factor(103, 104); - sfg.push_factor(104, 101); + symbolicGraph.push_factor(100); + symbolicGraph.push_factor(100, 101); + symbolicGraph.push_factor(101, 102); + symbolicGraph.push_factor(102, 103); + symbolicGraph.push_factor(103, 104); + symbolicGraph.push_factor(104, 101); - MetisIndex mi(sfg); + MetisIndex mi(symbolicGraph); vector xadjExpected, adjExpected; xadjExpected += 0, 1, 4, 6, 8, 10; @@ -172,17 +200,18 @@ TEST(Ordering, csr_format_3) { } /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, csr_format_4) { - SymbolicFactorGraph sfg; + SymbolicFactorGraph symbolicGraph; - sfg.push_factor(Symbol('x', 1)); - sfg.push_factor(Symbol('x', 1), Symbol('x', 2)); - sfg.push_factor(Symbol('x', 2), Symbol('x', 3)); - sfg.push_factor(Symbol('x', 3), Symbol('x', 4)); - sfg.push_factor(Symbol('x', 4), Symbol('x', 5)); - sfg.push_factor(Symbol('x', 5), Symbol('x', 6)); + symbolicGraph.push_factor(Symbol('x', 1)); + symbolicGraph.push_factor(Symbol('x', 1), Symbol('x', 2)); + symbolicGraph.push_factor(Symbol('x', 2), Symbol('x', 3)); + symbolicGraph.push_factor(Symbol('x', 3), Symbol('x', 4)); + symbolicGraph.push_factor(Symbol('x', 4), Symbol('x', 5)); + symbolicGraph.push_factor(Symbol('x', 5), Symbol('x', 6)); - MetisIndex mi(sfg); + MetisIndex mi(symbolicGraph); vector xadjExpected, adjExpected; xadjExpected += 0, 1, 3, 5, 7, 9, 10; @@ -195,28 +224,29 @@ TEST(Ordering, csr_format_4) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - Ordering metOrder = Ordering::Metis(sfg); + Ordering metOrder = Ordering::Metis(symbolicGraph); // Test different symbol types - sfg.push_factor(Symbol('l', 1)); - sfg.push_factor(Symbol('x', 1), Symbol('l', 1)); - sfg.push_factor(Symbol('x', 2), Symbol('l', 1)); - sfg.push_factor(Symbol('x', 3), Symbol('l', 1)); - sfg.push_factor(Symbol('x', 4), Symbol('l', 1)); + symbolicGraph.push_factor(Symbol('l', 1)); + symbolicGraph.push_factor(Symbol('x', 1), Symbol('l', 1)); + symbolicGraph.push_factor(Symbol('x', 2), Symbol('l', 1)); + symbolicGraph.push_factor(Symbol('x', 3), Symbol('l', 1)); + symbolicGraph.push_factor(Symbol('x', 4), Symbol('l', 1)); - Ordering metOrder2 = Ordering::Metis(sfg); + Ordering metOrder2 = Ordering::Metis(symbolicGraph); } - +#endif /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, metis) { - SymbolicFactorGraph sfg; + SymbolicFactorGraph symbolicGraph; - sfg.push_factor(0); - sfg.push_factor(0, 1); - sfg.push_factor(1, 2); + symbolicGraph.push_factor(0); + symbolicGraph.push_factor(0, 1); + symbolicGraph.push_factor(1, 2); - MetisIndex mi(sfg); + MetisIndex mi(symbolicGraph); vector xadjExpected, adjExpected; xadjExpected += 0, 1, 3, 4; @@ -226,22 +256,23 @@ TEST(Ordering, metis) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - Ordering metis = Ordering::Metis(sfg); + Ordering metis = Ordering::Metis(symbolicGraph); } - +#endif /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, MetisLoop) { // create linear graph - SymbolicFactorGraph sfg = example::symbolicChain(); + SymbolicFactorGraph symbolicGraph = example::symbolicChain(); // add loop closure - sfg.push_factor(0, 5); + symbolicGraph.push_factor(0, 5); // METIS #if !defined(__APPLE__) { - Ordering actual = Ordering::Create(Ordering::METIS, sfg); + Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); // - P( 0 4 1) // | - P( 2 | 4 1) // | | - P( 3 | 4 2) @@ -251,7 +282,7 @@ TEST(Ordering, MetisLoop) { } #else { - Ordering actual = Ordering::Create(Ordering::METIS, sfg); + Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); // - P( 1 0 3) // | - P( 4 | 0 3) // | | - P( 5 | 0 4) @@ -261,12 +292,12 @@ TEST(Ordering, MetisLoop) { } #endif } - +#endif /* ************************************************************************* */ TEST(Ordering, Create) { // create chain graph - SymbolicFactorGraph sfg = example::symbolicChain(); + SymbolicFactorGraph symbolicGraph = example::symbolicChain(); // COLAMD { @@ -275,23 +306,25 @@ TEST(Ordering, Create) { //| | - P( 2 | 3) //| | | - P( 1 | 2) //| | | | - P( 0 | 1) - Ordering actual = Ordering::Create(Ordering::COLAMD, sfg); + Ordering actual = Ordering::Create(Ordering::COLAMD, symbolicGraph); Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); EXPECT(assert_equal(expected, actual)); } +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION // METIS { - Ordering actual = Ordering::Create(Ordering::METIS, sfg); + Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); //- P( 1 0 2) //| - P( 3 4 | 2) //| | - P( 5 | 4) Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); EXPECT(assert_equal(expected, actual)); } +#endif // CUSTOM - CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, sfg), runtime_error); + CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, symbolicGraph), runtime_error); } /* ************************************************************************* */ diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h index 23d11964c..462258762 100644 --- a/gtsam/linear/BinaryJacobianFactor.h +++ b/gtsam/linear/BinaryJacobianFactor.h @@ -73,12 +73,12 @@ struct BinaryJacobianFactor: JacobianFactor { Eigen::Block b(Ab, 0, N1 + N2); // We perform I += A'*A to the upper triangle - (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); - (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2; - (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; - (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); - (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; - (*info)(slotB, slotB)(0, 0) += b.transpose() * b; + info->diagonalBlock(slot1).rankUpdate(A1.transpose()); + info->updateOffDiagonalBlock(slot1, slot2, A1.transpose() * A2); + info->updateOffDiagonalBlock(slot1, slotB, A1.transpose() * b); + info->diagonalBlock(slot2).rankUpdate(A2.transpose()); + info->updateOffDiagonalBlock(slot2, slotB, A2.transpose() * b); + info->updateDiagonalBlock(slotB, b.transpose() * b); } } }; diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index d40250266..41e2d8c84 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -17,7 +17,6 @@ * @author Christian Potthast */ -#include #include #include #include @@ -31,7 +30,7 @@ Errors::Errors(){} /* ************************************************************************* */ Errors::Errors(const VectorValues& V) { - BOOST_FOREACH(const Vector& e, V | boost::adaptors::map_values) { + for(const Vector& e: V | boost::adaptors::map_values) { push_back(e); } } @@ -39,7 +38,7 @@ Errors::Errors(const VectorValues& V) { /* ************************************************************************* */ void Errors::print(const std::string& s) const { cout << s << endl; - BOOST_FOREACH(const Vector& v, *this) + for(const Vector& v: *this) gtsam::print(v); } @@ -66,7 +65,7 @@ Errors Errors::operator+(const Errors& b) const { #endif Errors result; Errors::const_iterator it = b.begin(); - BOOST_FOREACH(const Vector& ai, *this) + for(const Vector& ai: *this) result.push_back(ai + *(it++)); return result; } @@ -81,7 +80,7 @@ Errors Errors::operator-(const Errors& b) const { #endif Errors result; Errors::const_iterator it = b.begin(); - BOOST_FOREACH(const Vector& ai, *this) + for(const Vector& ai: *this) result.push_back(ai - *(it++)); return result; } @@ -89,7 +88,7 @@ Errors Errors::operator-(const Errors& b) const { /* ************************************************************************* */ Errors Errors::operator-() const { Errors result; - BOOST_FOREACH(const Vector& ai, *this) + for(const Vector& ai: *this) result.push_back(-ai); return result; } @@ -105,7 +104,7 @@ double dot(const Errors& a, const Errors& b) { #endif double result = 0.0; Errors::const_iterator it = b.begin(); - BOOST_FOREACH(const Vector& ai, a) + for(const Vector& ai: a) result += gtsam::dot(ai, *(it++)); return result; } @@ -114,7 +113,7 @@ double dot(const Errors& a, const Errors& b) { template<> void axpy(double alpha, const Errors& x, Errors& y) { Errors::const_iterator it = x.begin(); - BOOST_FOREACH(Vector& yi, y) + for(Vector& yi: y) axpy(alpha,*(it++),yi); } diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 948daf2ad..c9ef922be 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -20,14 +20,11 @@ #include #include -#include +#include using namespace std; using namespace gtsam; -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) -#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL) - namespace gtsam { // Instantiate base class @@ -52,7 +49,7 @@ namespace gtsam { VectorValues soln(solutionForMissing); // possibly empty // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) /** solve each node in turn in topological sort order (parents first)*/ - BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { + for (auto cg: boost::adaptors::reverse(*this)) { // i^th part of R*x=y, x=inv(R)*y // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) soln.insert(cg->solve(soln)); @@ -88,7 +85,7 @@ namespace gtsam { VectorValues result; // TODO this looks pretty sketchy. result is passed as the parents argument // as it's filled up by solving the gaussian conditionals. - BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { + for (auto cg: boost::adaptors::reverse(*this)) { result.insert(cg->solveOtherRHS(result, rhs)); } return result; @@ -107,7 +104,7 @@ namespace gtsam { // we loop from first-eliminated to last-eliminated // i^th part of L*gy=gx is done block-column by block-column of L - BOOST_FOREACH(const sharedConditional& cg, *this) + for(const sharedConditional& cg: *this) cg->solveTransposeInPlace(gy); return gy; @@ -146,15 +143,15 @@ namespace gtsam { KeySet keys = factorGraph.keys(); // add frontal keys in order Ordering ordering; - BOOST_FOREACH (const sharedConditional& cg, *this) + for (const sharedConditional& cg: *this) if (cg) { - BOOST_FOREACH (Key key, cg->frontals()) { + for (Key key: cg->frontals()) { ordering.push_back(key); keys.erase(key); } } // add remaining keys in case Bayes net is incomplete - BOOST_FOREACH (Key key, keys) + for (Key key: keys) ordering.push_back(key); // return matrix and RHS return factorGraph.jacobian(ordering); @@ -182,7 +179,7 @@ namespace gtsam { double GaussianBayesNet::logDeterminant() const { double logDet = 0.0; - BOOST_FOREACH(const sharedConditional& cg, *this) { + for(const sharedConditional& cg: *this) { if(cg->get_model()) { Vector diag = cg->get_R().diagonal(); cg->get_model()->whitenInPlace(diag); diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index 1f6c8e9f5..ab311fdf2 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -19,8 +19,6 @@ #pragma once -#include - #include // Only to help Eclipse #include @@ -35,7 +33,7 @@ void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValue clique->conditional()->solveInPlace(result); // starting from the root, call optimize on each conditional - BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) + for(const typename BAYESTREE::sharedClique& child: clique->children_) optimizeInPlace(child, result); } @@ -48,7 +46,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) { result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); // sum of children - BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) + for(const typename BAYESTREE::sharedClique& child: clique->children_) result += logDeterminant(child); return result; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 19d6bd607..a4df04cf9 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -30,8 +30,6 @@ #include #include -#include - using namespace std; using namespace gtsam; @@ -50,7 +48,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { KeySet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) + for (const sharedFactor& factor: *this) if (factor) keys.insert(factor->begin(), factor->end()); return keys; @@ -59,8 +57,8 @@ namespace gtsam { /* ************************************************************************* */ std::map GaussianFactorGraph::getKeyDimMap() const { map spec; - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) { - for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) { + for (const GaussianFactor::shared_ptr& gf : *this) { + for (GaussianFactor::const_iterator it = gf->begin(); it != gf->end(); it++) { map::iterator it2 = spec.find(*it); if ( it2 == spec.end() ) { spec.insert(make_pair(*it, gf->getDim(it))); @@ -80,7 +78,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph GaussianFactorGraph::clone() const { GaussianFactorGraph result; - BOOST_FOREACH(const sharedFactor& f, *this) { + for (const sharedFactor& f : *this) { if (f) result.push_back(f->clone()); else @@ -92,9 +90,9 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph GaussianFactorGraph::negate() const { GaussianFactorGraph result; - BOOST_FOREACH(const sharedFactor& f, *this) { - if (f) - result.push_back(f->negate()); + for (const sharedFactor& factor: *this) { + if (factor) + result.push_back(factor->negate()); else result.push_back(sharedFactor()); // Passes on null factors so indices remain valid } @@ -106,8 +104,9 @@ namespace gtsam { // First find dimensions of each variable typedef std::map KeySizeMap; KeySizeMap dims; - BOOST_FOREACH(const sharedFactor& factor, *this) { - if (!static_cast(factor)) continue; + for (const sharedFactor& factor : *this) { + if (!static_cast(factor)) + continue; for (GaussianFactor::const_iterator key = factor->begin(); key != factor->end(); ++key) { @@ -118,7 +117,7 @@ namespace gtsam { // Compute first scalar column of each variable size_t currentColIndex = 0; KeySizeMap columnIndices = dims; - BOOST_FOREACH(const KeySizeMap::value_type& col, dims) { + for (const KeySizeMap::value_type& col : dims) { columnIndices[col.first] = currentColIndex; currentColIndex += dims[col.first]; } @@ -127,7 +126,7 @@ namespace gtsam { typedef boost::tuple triplet; vector entries; size_t row = 0; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for (const sharedFactor& factor : *this) { if (!static_cast(factor)) continue; // Convert to JacobianFactor if necessary @@ -211,25 +210,21 @@ namespace gtsam { // combine all factors and get upper-triangular part of Hessian Scatter scatter(*this, optionalOrdering); HessianFactor combined(*this, scatter); - Matrix result = combined.info(); - // Fill in lower-triangular part of Hessian - result.triangularView() = result.transpose(); - return result; + return combined.info().selfadjointView();; } /* ************************************************************************* */ pair GaussianFactorGraph::hessian( boost::optional optionalOrdering) const { Matrix augmented = augmentedHessian(optionalOrdering); - return make_pair( - augmented.topLeftCorner(augmented.rows() - 1, augmented.rows() - 1), - augmented.col(augmented.rows() - 1).head(augmented.rows() - 1)); + size_t n = augmented.rows() - 1; + return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1)); } /* ************************************************************************* */ VectorValues GaussianFactorGraph::hessianDiagonal() const { VectorValues d; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for (const sharedFactor& factor : *this) { if(factor){ VectorValues di = factor->hessianDiagonal(); d.addInPlace_(di); @@ -241,11 +236,11 @@ namespace gtsam { /* ************************************************************************* */ map GaussianFactorGraph::hessianBlockDiagonal() const { map blocks; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for (const sharedFactor& factor : *this) { if (!factor) continue; map BD = factor->hessianBlockDiagonal(); map::const_iterator it = BD.begin(); - for(;it!=BD.end();it++) { + for (;it!=BD.end();++it) { Key j = it->first; // variable key for this block const Matrix& Bj = it->second; if (blocks.count(j)) @@ -264,6 +259,30 @@ namespace gtsam { return BaseEliminateable::eliminateMultifrontal(ordering, function)->optimize(); } + /* ************************************************************************* */ + // TODO(frank): can we cache memory across invocations + VectorValues GaussianFactorGraph::optimizeDensely() const { + gttic(GaussianFactorGraph_optimizeDensely); + + // Combine all factors in a single HessianFactor (as done in augmentedHessian) + Scatter scatter(*this); + HessianFactor combined(*this, scatter); + + // TODO(frank): cast to large dynamic matrix :-( + // NOTE(frank): info only valid (I think) in upper triangle. No problem for LLT... + Matrix augmented = combined.info().selfadjointView(); + + // Do Cholesky Factorization + size_t n = augmented.rows() - 1; + auto AtA = augmented.topLeftCorner(n, n); + auto eta = augmented.topRightCorner(n, 1); + Eigen::LLT llt(AtA); + + // Solve and convert, re-using scatter data structure + Vector solution = llt.solve(eta); + return VectorValues(solution, scatter); + } + /* ************************************************************************* */ namespace { JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { @@ -279,8 +298,8 @@ namespace gtsam { VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const { VectorValues g = VectorValues::Zero(x0); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for (const sharedFactor& factor: *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor); Vector e = Ai->error_vector(x0); Ai->transposeMultiplyAdd(1.0, e, g); } @@ -291,7 +310,7 @@ namespace gtsam { VectorValues GaussianFactorGraph::gradientAtZero() const { // Zero-out the gradient VectorValues g; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for (const sharedFactor& factor: *this) { if (!factor) continue; VectorValues gi = factor->gradientAtZero(); g.addInPlace_(gi); @@ -331,8 +350,8 @@ namespace gtsam { /* ************************************************************************* */ Errors GaussianFactorGraph::operator*(const VectorValues& x) const { Errors e; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for (const GaussianFactor::shared_ptr& factor: *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor); e.push_back((*Ai) * x); } return e; @@ -341,7 +360,7 @@ namespace gtsam { /* ************************************************************************* */ void GaussianFactorGraph::multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + for (const GaussianFactor::shared_ptr& f: *this) f->multiplyHessianAdd(alpha, x, y); } @@ -353,8 +372,8 @@ namespace gtsam { /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const { Errors::iterator ei = e; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for (const GaussianFactor::shared_ptr& factor: *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor); *ei = (*Ai)*x; ei++; } @@ -363,7 +382,7 @@ namespace gtsam { /* ************************************************************************* */ bool hasConstraints(const GaussianFactorGraph& factors) { typedef JacobianFactor J; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + for (const GaussianFactor::shared_ptr& factor: factors) { J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) { return true; @@ -378,8 +397,8 @@ namespace gtsam { VectorValues& x) const { // For each factor add the gradient contribution Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for (const sharedFactor& factor: *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor); Ai->transposeMultiplyAdd(alpha, *(ei++), x); } } @@ -387,8 +406,8 @@ namespace gtsam { ///* ************************************************************************* */ //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { // Key i = 0 ; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + // for (const GaussianFactor::shared_ptr& factor, fg) { + // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor); // r[i] = Ai->getb(); // i++; // } @@ -401,10 +420,10 @@ namespace gtsam { //void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { // r.setZero(); // Key i = 0; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + // for (const GaussianFactor::shared_ptr& factor, fg) { + // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor); // Vector &y = r[i]; - // for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + // for (JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { // y += Ai->getA(j) * x[*j]; // } // ++i; @@ -416,9 +435,9 @@ namespace gtsam { { VectorValues x; Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + for (const sharedFactor& factor: *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor); + for (JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { // Create the value as a zero vector if it does not exist. pair xi = x.tryInsert(*j, Vector()); if(xi.second) @@ -434,8 +453,8 @@ namespace gtsam { Errors GaussianFactorGraph::gaussianErrors(const VectorValues& x) const { Errors e; - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for (const sharedFactor& factor: *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor); e.push_back(Ai->error_vector(x)); } return e; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index ae552adcd..49cba482d 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -144,7 +144,7 @@ namespace gtsam { /** unnormalized error */ double error(const VectorValues& x) const { double total_error = 0.; - BOOST_FOREACH(const sharedFactor& factor, *this){ + for(const sharedFactor& factor: *this){ if(factor) total_error += factor->error(x); } @@ -246,6 +246,11 @@ namespace gtsam { VectorValues optimize(OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + /** + * Optimize using Eigen's dense Cholesky factorization + */ + VectorValues optimizeDensely() const; + /** * Compute the gradient of the energy function, * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index 982c17bd3..b038e5452 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -23,7 +23,7 @@ namespace gtsam { // Instantiate base classes - template class ClusterTree; + template class EliminatableClusterTree; template class JunctionTree; /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianJunctionTree.h b/gtsam/linear/GaussianJunctionTree.h index f944d417b..67e5a374b 100644 --- a/gtsam/linear/GaussianJunctionTree.h +++ b/gtsam/linear/GaussianJunctionTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -26,21 +26,9 @@ namespace gtsam { class GaussianEliminationTree; /** - * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with - * the additional property that it represents the clique tree associated with a Bayes net. - * - * In GTSAM a junction tree is an intermediate data structure in multifrontal - * variable elimination. Each node is a cluster of factors, along with a - * clique of variables that are eliminated all at once. In detail, every node k represents - * a clique (maximal fully connected subset) of an associated chordal graph, such as a - * chordal Bayes net resulting from elimination. - * - * The difference with the BayesTree is that a JunctionTree stores factors, whereas a - * BayesTree stores conditionals, that are the product of eliminating the factors in the - * corresponding JunctionTree cliques. - * - * The tree structure and elimination method are exactly analagous to the EliminationTree, - * except that in the JunctionTree, at each node multiple variables are eliminated at a time. + * A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian + * factors stored in each cluster. It can be eliminated into a Gaussian Bayes tree with the same + * structure, which is essentially doing multifrontal sparse matrix factorization. * * \addtogroup Multifrontal * \nosubgrouping @@ -51,7 +39,7 @@ namespace gtsam { typedef JunctionTree Base; ///< Base class typedef GaussianJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/linear/HessianFactor-inl.h b/gtsam/linear/HessianFactor-inl.h index 4a21ab0ff..957ae4519 100644 --- a/gtsam/linear/HessianFactor-inl.h +++ b/gtsam/linear/HessianFactor-inl.h @@ -29,10 +29,10 @@ namespace gtsam { if((DenseIndex)Base::keys_.size() != augmentedInformation.nBlocks() - 1) throw std::invalid_argument( "Error in HessianFactor constructor input. Number of provided keys plus\n" - "one for the information vector must equal the number of provided matrix blocks."); + "one for the information vector must equal the number of provided matrix blocks. "); // Check RHS dimension - if(augmentedInformation(0, augmentedInformation.nBlocks() - 1).cols() != 1) + if(augmentedInformation.getDim(augmentedInformation.nBlocks() - 1) != 1) throw std::invalid_argument( "Error in HessianFactor constructor input. The last provided matrix block\n" "must be the information vector, but the last provided block had more than one column."); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 1c55d293b..29b2b8591 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -29,7 +29,6 @@ #include #include -#include #include #include #include @@ -58,10 +57,33 @@ using namespace boost::adaptors; namespace gtsam { +/* ************************************************************************* */ +void HessianFactor::Allocate(const Scatter& scatter) { + gttic(HessianFactor_Allocate); + + // Allocate with dimensions for each variable plus 1 at the end for the information vector + const size_t n = scatter.size(); + keys_.resize(n); + FastVector dims(n + 1); + DenseIndex slot = 0; + for(const SlotEntry& slotentry: scatter) { + keys_[slot] = slotentry.key; + dims[slot] = slotentry.dimension; + ++slot; + } + dims.back() = 1; + info_ = SymmetricBlockMatrix(dims); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const Scatter& scatter) { + Allocate(scatter); +} + /* ************************************************************************* */ HessianFactor::HessianFactor() : info_(cref_list_of<1>(1)) { - linearTerm().setZero(); + assert(info_.rows() == 1); constantTerm() = 0.0; } @@ -71,9 +93,9 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) if (G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0, 0) = G; - info_(0, 1) = g; - info_(1, 1)(0, 0) = f; + info_.setDiagonalBlock(0, G); + linearTerm() = g; + constantTerm() = f; } /* ************************************************************************* */ @@ -84,9 +106,9 @@ HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0, 0) = Sigma.inverse(); // G - info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g - info_(1, 1)(0, 0) = mu.dot(info_(0, 1).knownOffDiagonal().col(0)); // f + info_.setDiagonalBlock(0, Sigma.inverse()); // G + linearTerm() = info_.diagonalBlock(0) * mu; // g + constantTerm() = mu.dot(linearTerm().col(0)); // f } /* ************************************************************************* */ @@ -95,12 +117,11 @@ HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, double f) : GaussianFactor(cref_list_of<2>(j1)(j2)), info_( cref_list_of<3>(G11.cols())(G22.cols())(1)) { - info_(0, 0) = G11; - info_(0, 1) = G12; - info_(0, 2) = g1; - info_(1, 1) = G22; - info_(1, 2) = g2; - info_(2, 2)(0, 0) = f; + info_.setDiagonalBlock(0, G11); + info_.setOffDiagonalBlock(0, 1, G12); + info_.setDiagonalBlock(1, G22); + linearTerm() << g1, g2; + constantTerm() = f; } /* ************************************************************************* */ @@ -116,16 +137,14 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, || G22.cols() != g2.size() || G33.cols() != g3.size()) throw invalid_argument( "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - info_(0, 0) = G11; - info_(0, 1) = G12; - info_(0, 2) = G13; - info_(0, 3) = g1; - info_(1, 1) = G22; - info_(1, 2) = G23; - info_(1, 3) = g2; - info_(2, 2) = G33; - info_(2, 3) = g3; - info_(3, 3)(0, 0) = f; + info_.setDiagonalBlock(0, G11); + info_.setOffDiagonalBlock(0, 1, G12); + info_.setOffDiagonalBlock(0, 2, G13); + info_.setDiagonalBlock(1, G22); + info_.setOffDiagonalBlock(1, 2, G23); + info_.setDiagonalBlock(2, G33); + linearTerm() << g1, g2, g3; + constantTerm() = f; } /* ************************************************************************* */ @@ -175,11 +194,16 @@ HessianFactor::HessianFactor(const std::vector& js, size_t index = 0; for (size_t i = 0; i < variable_count; ++i) { for (size_t j = i; j < variable_count; ++j) { - info_(i, j) = Gs[index++]; + if (i == j) { + info_.setDiagonalBlock(i, Gs[index]); + } else { + info_.setOffDiagonalBlock(i, j, Gs[index]); + } + index++; } - info_(i, variable_count) = gs[i]; + info_.setOffDiagonalBlock(i, variable_count, gs[i]); } - info_(variable_count, variable_count)(0, 0) = f; + constantTerm() = f; } /* ************************************************************************* */ @@ -187,17 +211,17 @@ namespace { void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) { gttic(HessianFactor_fromJacobian); const SharedDiagonal& jfModel = jf.get_model(); + auto A = jf.matrixObject().full(); if (jfModel) { if (jf.get_model()->isConstrained()) throw invalid_argument( "Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - info.full().triangularView() = - jf.matrixObject().full().transpose() - * (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() - * jf.matrixObject().full(); + + auto D = (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal(); + + info.setFullMatrix(A.transpose() * D * A); } else { - info.full().triangularView() = jf.matrixObject().full().transpose() - * jf.matrixObject().full(); + info.setFullMatrix(A.transpose() * A); } } } @@ -228,32 +252,13 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : HessianFactor::HessianFactor(const GaussianFactorGraph& factors, boost::optional scatter) { gttic(HessianFactor_MergeConstructor); - boost::optional computedScatter; - if (!scatter) { - computedScatter = Scatter(factors); - scatter = computedScatter; - } - // Allocate and copy keys - gttic(allocate); - // Allocate with dimensions for each variable plus 1 at the end for the information vector - const size_t n = scatter->size(); - keys_.resize(n); - FastVector dims(n + 1); - DenseIndex slot = 0; - BOOST_FOREACH(const SlotEntry& slotentry, *scatter) { - keys_[slot] = slotentry.key; - dims[slot] = slotentry.dimension; - ++slot; - } - dims.back() = 1; - info_ = SymmetricBlockMatrix(dims); - info_.full().triangularView().setZero(); - gttoc(allocate); + Allocate(scatter ? *scatter : Scatter(factors)); // Form A' * A gttic(update); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) + info_.setZero(); + for(const auto& factor: factors) if (factor) factor->updateHessian(keys_, &info_); gttoc(update); @@ -267,7 +272,7 @@ void HessianFactor::print(const std::string& s, for (const_iterator key = begin(); key != end(); ++key) cout << formatter(*key) << "(" << getDim(key) << ") "; cout << "\n"; - gtsam::print(Matrix(info_.full().selfadjointView()), + gtsam::print(Matrix(info_.selfadjointView()), "Augmented information matrix: "); } @@ -282,22 +287,25 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { /* ************************************************************************* */ Matrix HessianFactor::augmentedInformation() const { - return info_.full().selfadjointView(); + return info_.selfadjointView(); +} + +/* ************************************************************************* */ +Eigen::SelfAdjointView +HessianFactor::informationView() const { + return info_.selfadjointView(0, size()); } /* ************************************************************************* */ Matrix HessianFactor::information() const { - return info_.range(0, size(), 0, size()).selfadjointView(); + return informationView(); } /* ************************************************************************* */ VectorValues HessianFactor::hessianDiagonal() const { VectorValues d; - // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - // Get the diagonal block, and insert its diagonal - Matrix B = info_(j, j).selfadjointView(); - d.insert(keys_[j], B.diagonal()); + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + d.insert(keys_[j], info_.diagonal(j)); } return d; } @@ -315,8 +323,7 @@ map HessianFactor::hessianBlockDiagonal() const { // Loop over all variables for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert it - Matrix B = info_(j, j).selfadjointView(); - blocks.insert(make_pair(keys_[j], B)); + blocks.emplace(keys_[j], info_.diagonalBlock(j)); } return blocks; } @@ -335,28 +342,44 @@ std::pair HessianFactor::jacobian() const { double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) const double f = constantTerm(); + if (empty()) { + return 0.5 * f; + } double xtg = 0, xGx = 0; // extract the relevant subset of the VectorValues // NOTE may not be as efficient const Vector x = c.vector(keys()); - xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x; + xtg = x.dot(linearTerm().col(0)); + auto AtA = informationView(); + xGx = x.transpose() * AtA * x; return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ void HessianFactor::updateHessian(const FastVector& infoKeys, - SymmetricBlockMatrix* info) const { + SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); + assert(info); // Apply updates to the upper triangle - DenseIndex n = size(), N = info->nBlocks() - 1; - vector slots(n + 1); - for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + DenseIndex nrVariablesInThisFactor = size(), nrBlocksInInfo = info->nBlocks() - 1; + vector slots(nrVariablesInThisFactor + 1); + // Loop over this factor's blocks with indices (i,j) + // For every block (i,j), we determine the block (I,J) in info. + for (DenseIndex j = 0; j <= nrVariablesInThisFactor; ++j) { + const bool rhs = (j == nrVariablesInThisFactor); + const DenseIndex J = rhs ? nrBlocksInInfo : Slot(infoKeys, keys_[j]); slots[j] = J; for (DenseIndex i = 0; i <= j; ++i) { - const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. - (*info)(I, J) += info_(i, j); + const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. + + if (i == j) { + assert(I == J); + info->updateDiagonalBlock(I, info_.diagonalBlock(i)); + } else { + assert(i < j); + assert(I != J); + info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j)); + } } } } @@ -364,8 +387,8 @@ void HessianFactor::updateHessian(const FastVector& infoKeys, /* ************************************************************************* */ GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = boost::make_shared(*this); - result->info_.full().triangularView() = - -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result + // Negate the information matrix of the result + result->info_.negate(); return result; } @@ -387,12 +410,13 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, Vector xj = x.at(keys_[j]); DenseIndex i = 0; for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() * xj; + y[i] += info_.aboveDiagonalBlock(i, j) * xj; + // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() * xj; + y[i] += info_.diagonalBlock(j) * xj; // for below diagonal, we take transpose block from upper triangular part for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() * xj; + y[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj; } // copy to yvalues @@ -412,7 +436,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j], -info_(j, n).knownOffDiagonal()); + g.insert(keys_[j], -info_.aboveDiagonalBlock(j, n)); return g; } @@ -425,31 +449,80 @@ void HessianFactor::gradientAtZero(double* d) const { /* ************************************************************************* */ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { - Factor::const_iterator i = find(key); + const Factor::const_iterator it_i = find(key); + const DenseIndex I = std::distance(begin(), it_i); + // Sum over G_ij*xj for all xj connecting to xi Vector b = Vector::Zero(x.at(key).size()); - for (Factor::const_iterator j = begin(); j != end(); ++j) { + for (Factor::const_iterator it_j = begin(); it_j != end(); ++it_j) { + const DenseIndex J = std::distance(begin(), it_j); + // Obtain Gij from the Hessian factor // Hessian factor only stores an upper triangular matrix, so be careful when i>j - Matrix Gij; - if (i > j) { - Matrix Gji = info(j, i); - Gij = Gji.transpose(); - } else { - Gij = info(i, j); - } + const Matrix Gij = info_.block(I, J); // Accumulate Gij*xj to gradf - b += Gij * x.at(*j); + b += Gij * x.at(*it_j); } // Subtract the linear term gi - b += -linearTerm(i); + b += -linearTerm(it_i); return b; } /* ************************************************************************* */ -std::pair, - boost::shared_ptr > EliminateCholesky( - const GaussianFactorGraph& factors, const Ordering& keys) { +boost::shared_ptr HessianFactor::eliminateCholesky(const Ordering& keys) { + gttic(HessianFactor_eliminateCholesky); + + GaussianConditional::shared_ptr conditional; + + try { + // Do dense elimination + size_t nFrontals = keys.size(); + assert(nFrontals <= size()); + info_.choleskyPartial(nFrontals); + + // TODO(frank): pre-allocate GaussianConditional and write into it + const VerticalBlockMatrix Ab = info_.split(nFrontals); + conditional = boost::make_shared(keys_, nFrontals, Ab); + + // Erase the eliminated keys in this factor + keys_.erase(begin(), begin() + nFrontals); + } catch (const CholeskyFailed& e) { + throw IndeterminantLinearSystemException(keys.front()); + } + + // Return result + return conditional; +} + +/* ************************************************************************* */ +VectorValues HessianFactor::solve() { + gttic(HessianFactor_solve); + + // Do Cholesky Factorization + const size_t n = size(); + assert(info_.nBlocks() == n + 1); + info_.choleskyPartial(n); + auto R = info_.triangularView(0, n); + auto eta = linearTerm(); + + // Solve + const Vector solution = R.solve(eta); + + // Parse into VectorValues + VectorValues delta; + std::size_t offset = 0; + for (DenseIndex j = 0; j < (DenseIndex)n; ++j) { + const DenseIndex dim = info_.getDim(j); + delta.insert(keys_[j], solution.segment(offset, dim)); + offset += dim; + } + + return delta; +} + +/* ************************************************************************* */ +std::pair, boost::shared_ptr > +EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateCholesky); // Build joint factor @@ -460,23 +533,11 @@ std::pair, } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateCholesky was called with a request to eliminate variables that are not\n" - "involved in the provided factors."); + "involved in the provided factors."); } // Do dense elimination - GaussianConditional::shared_ptr conditional; - try { - size_t numberOfKeysToEliminate = keys.size(); - VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial( - numberOfKeysToEliminate); - conditional = boost::make_shared(jointFactor->keys(), - numberOfKeysToEliminate, Ab); - // Erase the eliminated keys in the remaining factor - jointFactor->keys_.erase(jointFactor->begin(), - jointFactor->begin() + numberOfKeysToEliminate); - } catch (const CholeskyFailed& e) { - throw IndeterminantLinearSystemException(keys.front()); - } + auto conditional = jointFactor->eliminateCholesky(keys); // Return result return make_pair(conditional, jointFactor); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index b74d557ea..e28bcdd81 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -35,12 +35,6 @@ namespace gtsam { class GaussianBayesNet; class GaussianFactorGraph; - GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys); - - GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); - /** * @brief A Gaussian factor using the canonical parameters (information form) * @@ -206,7 +200,9 @@ namespace gtsam { * @param variable An iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. */ - virtual DenseIndex getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } + virtual DenseIndex getDim(const_iterator variable) const { + return info_.getDim(std::distance(begin(), variable)); + } /** Return the number of columns and rows of the Hessian matrix, including the information vector. */ size_t rows() const { return info_.rows(); } @@ -217,80 +213,54 @@ namespace gtsam { * @return a HessianFactor with negated Hessian matrices */ virtual GaussianFactor::shared_ptr negate() const; - + /** Check if the factor is empty. TODO: How should this be defined? */ virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } - /** Return a view of the block at (j1,j2) of the upper-triangular part of the - * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation - * above to explain that only the upper-triangular part of the information matrix is stored - * and returned by this function. - * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return A view of the requested block, not a copy. + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ */ - constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } - - /** Return a view of the block at (j1,j2) of the upper-triangular part of the - * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation - * above to explain that only the upper-triangular part of the information matrix is stored - * and returned by this function. - * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return A view of the requested block, not a copy. - */ - Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); } - - /** Return the upper-triangular part of the full *augmented* information matrix, - * as described above. See HessianFactor class documentation above to explain that only the - * upper-triangular part of the information matrix is stored and returned by this function. - */ - SymmetricBlockMatrix::constBlock info() const { return info_.full(); } - - /** Return the upper-triangular part of the full *augmented* information matrix, - * as described above. See HessianFactor class documentation above to explain that only the - * upper-triangular part of the information matrix is stored and returned by this function. - */ - SymmetricBlockMatrix::Block info() { return info_.full(); } + double constantTerm() const { + const auto view = info_.diagonalBlock(size()); + return view(0, 0); + } /** Return the constant term \f$ f \f$ as described above * @return The constant term \f$ f \f$ */ - double constantTerm() const { return info_(this->size(), this->size())(0,0); } - - /** Return the constant term \f$ f \f$ as described above - * @return The constant term \f$ f \f$ - */ - double& constantTerm() { return info_(this->size(), this->size())(0,0); } + double& constantTerm() { return info_.diagonalBlock(size())(0, 0); } /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ - constBlock::OffDiagonal::ColXpr linearTerm(const_iterator j) const { - return info_(j-begin(), size()).knownOffDiagonal().col(0); } - - /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. - * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return The linear term \f$ g \f$ */ - Block::OffDiagonal::ColXpr linearTerm(iterator j) { - return info_(j-begin(), size()).knownOffDiagonal().col(0); } + SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const { + assert(!empty()); + return info_.aboveDiagonalBlock(j - begin(), size()); + } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - constBlock::OffDiagonal::ColXpr linearTerm() const { - return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); } + SymmetricBlockMatrix::constBlock linearTerm() const { + assert(!empty()); + // get the last column (except the bottom right block) + return info_.aboveDiagonalRange(0, size(), size(), size() + 1); + } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - Block::OffDiagonal::ColXpr linearTerm() { - return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); } - + SymmetricBlockMatrix::Block linearTerm() { + assert(!empty()); + return info_.aboveDiagonalRange(0, size(), size(), size() + 1); + } + + /// Return underlying information matrix. + const SymmetricBlockMatrix& info() const { return info_; } + + /// Return non-const information matrix. + /// TODO(gareth): Review the sanity of having non-const access to this. + SymmetricBlockMatrix& info() { return info_; } + /** Return the augmented information matrix represented by this GaussianFactor. * The augmented information matrix contains the information matrix with an * additional column holding the information vector, and an additional row @@ -308,6 +278,9 @@ namespace gtsam { */ virtual Matrix augmentedInformation() const; + /// Return self-adjoint view onto the information matrix (NOT augmented). + Eigen::SelfAdjointView informationView() const; + /** Return the non-augmented information matrix represented by this * GaussianFactor. */ @@ -322,11 +295,7 @@ namespace gtsam { /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; - /** - * Return (dense) matrix associated with factor - * @param ordering of variables needed for matrix column order - * @param set weight to true to bake in the weights - */ + /// Return (dense) matrix associated with factor virtual std::pair jacobian() const; /** @@ -336,16 +305,21 @@ namespace gtsam { */ virtual Matrix augmentedJacobian() const; - /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */ - const SymmetricBlockMatrix& matrixObject() const { return info_; } - /** Update an information matrix by adding the information corresponding to this factor * (used internally during elimination). - * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param keys THe ordered vector of keys for the information matrix to be updated * @param info The information matrix to be updated */ void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; + /** Update another Hessian factor + * @param other the HessianFactor to be updated + */ + void updateHessian(HessianFactor* other) const { + assert(other); + updateHessian(other->keys_, &other->info_); + } + /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; @@ -362,44 +336,33 @@ namespace gtsam { Vector gradient(Key key, const VectorValues& x) const; /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors are - * left-multiplied with their transpose to form the Hessian using the conversion constructor - * HessianFactor(const JacobianFactor&). - * - * If any factors contain constrained noise models, this function will fail because our current - * implementation cannot handle constrained noise models in Cholesky factorization. The - * function EliminatePreferCholesky() automatically does QR instead when this is the case. - * - * Variables are eliminated in the order specified in \c keys. - * - * @param factors Factors to combine and eliminate - * @param keys The variables to eliminate and their elimination ordering - * @return The conditional and remaining factor - * - * \addtogroup LinearSolving */ - friend GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + * In-place elimination that returns a conditional on (ordered) keys specified, and leaves + * this factor to be on the remaining keys (separator) only. Does dense partial Cholesky. + */ + boost::shared_ptr eliminateCholesky(const Ordering& keys); - /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors are - * left-multiplied with their transpose to form the Hessian using the conversion constructor - * HessianFactor(const JacobianFactor&). - * - * This function will fall back on QR factorization for any cliques containing JacobianFactor's - * with constrained noise models. - * - * Variables are eliminated in the order specified in \c keys. - * - * @param factors Factors to combine and eliminate - * @param keys The variables to eliminate and their elimination ordering - * @return The conditional and remaining factor - * - * \addtogroup LinearSolving */ - friend GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + /// Solve the system A'*A delta = A'*b in-place, return delta as VectorValues + VectorValues solve(); + + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + const SymmetricBlockMatrix& matrixObject() const { return info_; } + /// @} +#endif private: + /// Allocate for given scatter pattern + void Allocate(const Scatter& scatter); + + /// Constructor with given scatter pattern, allocating but not initializing storage. + HessianFactor(const Scatter& scatter); + + friend class NonlinearFactorGraph; + friend class NonlinearClusterTree; + /** Serialization function */ friend class boost::serialization::access; template @@ -409,6 +372,43 @@ namespace gtsam { } }; +/** +* Densely partially eliminate with Cholesky factorization. JacobianFactors are +* left-multiplied with their transpose to form the Hessian using the conversion constructor +* HessianFactor(const JacobianFactor&). +* +* If any factors contain constrained noise models, this function will fail because our current +* implementation cannot handle constrained noise models in Cholesky factorization. The +* function EliminatePreferCholesky() automatically does QR instead when this is the case. +* +* Variables are eliminated in the order specified in \c keys. +* +* @param factors Factors to combine and eliminate +* @param keys The variables to eliminate and their elimination ordering +* @return The conditional and remaining factor +* +* \addtogroup LinearSolving */ +GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + +/** +* Densely partially eliminate with Cholesky factorization. JacobianFactors are +* left-multiplied with their transpose to form the Hessian using the conversion constructor +* HessianFactor(const JacobianFactor&). +* +* This function will fall back on QR factorization for any cliques containing JacobianFactor's +* with constrained noise models. +* +* Variables are eliminated in the order specified in \c keys. +* +* @param factors Factors to combine and eliminate +* @param keys The variables to eliminate and their elimination ordering +* @return The conditional and remaining factor +* +* \addtogroup LinearSolving */ +GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 79ade1c8a..411feb7a9 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include using namespace std; @@ -127,7 +126,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) { /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { + for ( const KeyInfo::value_type &item: *this ) { result[item.second.index()] = item.second.dim(); } return result; @@ -136,7 +135,7 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { + for ( const KeyInfo::value_type &item: *this ) { result.insert(item.first, Vector::Zero(item.second.dim())); } return result; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 7fecefe3c..4597759e3 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -32,7 +32,6 @@ #include #include -#include #include #include #include @@ -105,10 +104,10 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, /* ************************************************************************* */ JacobianFactor::JacobianFactor(const HessianFactor& factor) : Base(factor), Ab_( - VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(), + VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) { // Copy Hessian into our matrix and then do in-place Cholesky - Ab_.full() = factor.matrixObject().full(); + Ab_.full() = factor.info().selfadjointView(); // Do Cholesky to get a Jacobian size_t maxrank; @@ -185,12 +184,12 @@ boost::tuple, DenseIndex, DenseIndex> _countDims( "Unable to determine dimensionality for all variables"); } - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { + for(const JacobianFactor::shared_ptr& factor: factors) { m += factor->rows(); } #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(DenseIndex d, varDims) { + for(DenseIndex d: varDims) { assert(d != numeric_limits::max()); } #endif @@ -204,7 +203,7 @@ FastVector _convertOrCastToJacobians( gttic(Convert_to_Jacobians); FastVector jacobians; jacobians.reserve(factors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + for(const GaussianFactor::shared_ptr& factor: factors) { if (factor) { if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< JacobianFactor>(factor)) @@ -220,15 +219,13 @@ FastVector _convertOrCastToJacobians( /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, boost::optional ordering, - boost::optional variableSlots) { + boost::optional p_variableSlots) { gttic(JacobianFactor_combine_constructor); // Compute VariableSlots if one was not provided - boost::optional computedVariableSlots; - if (!variableSlots) { - computedVariableSlots = VariableSlots(graph); - variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots - } + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = + p_variableSlots ? p_variableSlots.get() : VariableSlots(graph); // Cast or convert to Jacobians FastVector jacobians = _convertOrCastToJacobians( @@ -239,15 +236,15 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then // be added after all of the ordered variables. FastVector orderedSlots; - orderedSlots.reserve(variableSlots->size()); + orderedSlots.reserve(variableSlots.size()); if (ordering) { // If an ordering is provided, arrange the slots first that ordering FastList unorderedSlots; size_t nOrderingSlotsUsed = 0; orderedSlots.resize(ordering->size()); FastMap inverseOrdering = ordering->invert(); - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) { + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) { FastMap::const_iterator orderingPosition = inverseOrdering.find(item->first); if (orderingPosition == inverseOrdering.end()) { @@ -262,14 +259,14 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, "The ordering provided to the JacobianFactor combine constructor\n" "contained extra variables that did not appear in the factors to combine."); // Add the remaining slots - BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots) { + for(VariableSlots::const_iterator item: unorderedSlots) { orderedSlots.push_back(item); } } else { // If no ordering is provided, arrange the slots as they were, which will be sorted // numerically since VariableSlots uses a map sorting on Key. - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) orderedSlots.push_back(item); } gttoc(Order_slots); @@ -292,7 +289,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, // Loop over slots in combined factor and copy blocks from source factors gttic(copy_blocks); size_t combinedSlot = 0; - BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) { + for(VariableSlots::const_iterator varslot: orderedSlots) { JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot)); // Loop over source jacobians DenseIndex nextRow = 0; @@ -533,10 +530,10 @@ void JacobianFactor::updateHessian(const FastVector& infoKeys, // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { const DenseIndex I = slots[i]; // because iupdateOffDiagonalBlock(I, J, Ab_(i).transpose() * Ab_j); } // Fill diagonal block with Aj'*Aj - (*info)(J, J).selfadjointView().rankUpdate(Ab_j.transpose()); + info->diagonalBlock(J).rankUpdate(Ab_j.transpose()); } } } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 3795f096e..d7814f541 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -37,6 +37,11 @@ namespace gtsam { class Ordering; class JacobianFactor; + /** + * Multiply all factors and eliminate the given keys from the resulting factor using a QR + * variant that handles constraints (zero sigmas). Computation happens in noiseModel::Gaussian::QR + * Returns a conditional on those keys, and a new factor on the separator. + */ GTSAM_EXPORT std::pair, boost::shared_ptr > EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); @@ -149,7 +154,7 @@ namespace gtsam { explicit JacobianFactor( const GaussianFactorGraph& graph, boost::optional ordering = boost::none, - boost::optional variableSlots = boost::none); + boost::optional p_variableSlots = boost::none); /** Virtual destructor */ virtual ~JacobianFactor() {} @@ -178,12 +183,12 @@ namespace gtsam { * which in fact stores an augmented information matrix. */ virtual Matrix augmentedInformation() const; - + /** Return the non-augmented information matrix represented by this * GaussianFactor. */ virtual Matrix information() const; - + /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; @@ -197,7 +202,7 @@ namespace gtsam { * @brief Returns (dense) A,b pair associated with factor, bakes in the weights */ virtual std::pair jacobian() const; - + /** * @brief Returns (dense) A,b pair associated with factor, does not bake in weights */ @@ -319,7 +324,7 @@ namespace gtsam { /** set noiseModel correctly */ void setModel(bool anyConstrained, const Vector& sigmas); - + /** * Densely partially eliminate with QR factorization, this is usually provided as an argument to * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors @@ -348,7 +353,7 @@ namespace gtsam { /// Internal function to fill blocks and set dimensions template void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); - + private: /** Unsafe Constructor that creates an uninitialized Jacobian of right size diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 5bcf3d635..4d9c7883b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #include #include @@ -211,7 +210,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { } void Gaussian::WhitenSystem(vector& A, Vector& b) const { - BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); } + for(Matrix& Aj: A) { WhitenInPlace(Aj); } whitenInPlace(b); } @@ -542,7 +541,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { size_t i = 0; // start with first row bool mixed = false; Ab.setZero(); // make sure we don't look below - BOOST_FOREACH (const Triple& t, Rd) { + for (const Triple& t: Rd) { const size_t& j = t.get<0>(); const Matrix& rd = t.get<1>(); precisions(i) = t.get<2>(); @@ -647,14 +646,14 @@ void Base::reweight(Vector& error) const { void Base::reweight(vector &A, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); - BOOST_FOREACH(Matrix& Aj, A) { + for(Matrix& Aj: A) { Aj *= w; } error *= w; } else { const Vector W = sqrtWeight(error); - BOOST_FOREACH(Matrix& Aj, A) { + for(Matrix& Aj: A) { vector_scale_inplace(W,Aj); } error = W.cwiseProduct(error); @@ -887,6 +886,30 @@ DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { return shared_ptr(new DCS(c, reweight)); } +/* ************************************************************************* */ +// L2WithDeadZone +/* ************************************************************************* */ + +L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { + if (k_ <= 0) { + throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor."); + } +} + +void L2WithDeadZone::print(const std::string &s="") const { + std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; +} + +bool L2WithDeadZone::equals(const Base &expected, double tol) const { + const L2WithDeadZone* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(k_ - p->k_) < tol; +} + +L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) { + return shared_ptr(new L2WithDeadZone(k, reweight)); +} + } // namespace mEstimator /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 3a8a6744c..e495921c2 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -662,7 +662,29 @@ namespace gtsam { Base(const ReweightScheme reweight = Block):reweight_(reweight) {} virtual ~Base() {} - /// robust error function to implement + /* + * This method is responsible for returning the total penalty for a given amount of error. + * For example, this method is responsible for implementing the quadratic function for an + * L2 penalty, the absolute value function for an L1 penalty, etc. + * + * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes + * implement a residual function, and GTSAM calls the weight function to evaluate the + * total penalty, rather than calling the residual function. The weight function should be + * used during iteratively reweighted least squares optimization, but should not be used to + * evaluate the total penalty. The long-term solution is for all mEstimators to implement + * both a weight and a residual function, and for GTSAM to call the residual function when + * evaluating the total penalty. But for now, I'm leaving this residual method as pure + * virtual, so the existing mEstimators can inherit this default fallback behavior. + */ + virtual double residual(double error) const { return 0; }; + + /* + * This method is responsible for returning the weight function for a given amount of error. + * The weight function is related to the analytic derivative of the residual function. See + * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * for details. This method is required when optimizing cost functions with robust penalties + * using iteratively re-weighted least squares. + */ virtual double weight(double error) const = 0; virtual void print(const std::string &s) const = 0; @@ -916,6 +938,45 @@ namespace gtsam { } }; + /// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k, + /// centered at the origin. The resulting penalty within the dead zone is always zero, and + /// grows quadratically outside the dead zone. In this sense, the L2WithDeadZone penalty is + /// "robust to inliers", rather than being robust to outliers. This penalty can be used to + /// create barrier functions in a general way. + class GTSAM_EXPORT L2WithDeadZone : public Base { + protected: + double k_; + + public: + typedef boost::shared_ptr shared_ptr; + + L2WithDeadZone(double k, const ReweightScheme reweight = Block); + double residual(double error) const { + const double abs_error = fabs(error); + return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); + } + double weight(double error) const { + // note that this code is slightly uglier than above, because there are three distinct + // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two + // cases (deadzone, non-deadzone) above. + if (fabs(error) <= k_) return 0.0; + else if (error > k_) return (-k_+error)/error; + else return (k_+error)/error; + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(k_); + } + }; + } ///\namespace mEstimator /** @@ -976,7 +1037,9 @@ namespace gtsam { { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } inline virtual double distance(const Vector& v) const { return this->whiten(v).squaredNorm(); } - + // TODO(mike): fold the use of the m-estimator residual(...) function into distance(...) + inline virtual double distance_non_whitened(const Vector& v) const + { return robust_->residual(v.norm()); } // TODO: these are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(std::vector& A, Vector& b) const; @@ -997,7 +1060,7 @@ namespace gtsam { ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); } }; - + // Helper function GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix M); diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index caf7d0095..f9ef756f1 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -150,7 +149,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, /**********************************************************************************/ VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; - BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { + for ( const KeyInfo::value_type &item: keyInfo ) { result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); } diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 538d886b4..d83385d32 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build( /* getting the block diagonals over the factors */ std::map hessianMap =gfg.hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + for ( const Matrix hessian: hessianMap | boost::adaptors::map_values) blocks.push_back(hessian); /* if necessary, allocating the memory for cacheing the factorization results */ diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h index e5e545c36..f4df9d96b 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -20,7 +20,6 @@ #include #include -#include #include namespace gtsam { @@ -114,7 +113,7 @@ public: double* yvalues) const { // Create a vector of temporary y_ values, corresponding to rows i y_.resize(size()); - BOOST_FOREACH(VectorD & yi, y_) + for(VectorD & yi: y_) yi.setZero(); // Accessing the VectorValues one by one is expensive @@ -126,12 +125,12 @@ public: const double* xj = x + key * D; DenseIndex i = 0; for (; i < j; ++i) - y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(xj); // blocks on the diagonal are only half - y_[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + y_[i] += info_.diagonalBlock(j) * ConstDMap(xj); // for below diagonal, we take transpose block from upper triangular part for (i = j + 1; i < (DenseIndex) size(); ++i) - y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(xj); } // copy to yvalues @@ -147,7 +146,7 @@ public: // Create a vector of temporary y_ values, corresponding to rows i y_.resize(size()); - BOOST_FOREACH(VectorD & yi, y_) + for(VectorD & yi: y_) yi.setZero(); // Accessing the VectorValues one by one is expensive @@ -156,16 +155,16 @@ public: for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DenseIndex i = 0; for (; i < j; ++i) - y_[i] += info_(i, j).knownOffDiagonal() + y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); // blocks on the diagonal are only half - y_[i] += info_(j, j).selfadjointView() + y_[i] += info_.diagonalBlock(j) * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); // for below diagonal, we take transpose block from upper triangular part for (i = j + 1; i < (DenseIndex) size(); ++i) - y_[i] += info_(i, j).knownOffDiagonal() + y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); } @@ -183,8 +182,7 @@ public: for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - const Matrix& B = info_(pos, pos).selfadjointView(); - DMap(d + D * j) += B.diagonal(); + DMap(d + D * j) += info_.diagonal(pos); } } @@ -195,8 +193,7 @@ public: for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - VectorD dj = -info_(pos, size()).knownOffDiagonal(); - DMap(d + D * j) += dj; + DMap(d + D * j) -= info_.aboveDiagonalBlock(pos, size());; } } diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h index bbc34d482..2b5cf85ff 100644 --- a/gtsam/linear/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -20,7 +20,6 @@ #include #include -#include namespace gtsam { diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index ed2af529f..db5c25442 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -20,7 +20,6 @@ #include #include -#include #include using namespace std; @@ -41,14 +40,15 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, // If we have an ordering, pre-fill the ordered variables first if (ordering) { - BOOST_FOREACH (Key key, *ordering) { - push_back(SlotEntry(key, 0)); + for (Key key : *ordering) { + add(key, 0); } } // Now, find dimensions of variables and/or extend - BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { - if (!factor) continue; + for (const auto& factor : gfg) { + if (!factor) + continue; // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers const JacobianFactor* asJacobian = dynamic_cast(factor.get()); @@ -62,7 +62,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, if (it!=end()) it->dimension = factor->getDim(variable); else - push_back(SlotEntry(key, factor->getDim(variable))); + add(key, factor->getDim(variable)); } } @@ -75,6 +75,11 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); } +/* ************************************************************************* */ +void Scatter::add(Key key, size_t dim) { + emplace_back(SlotEntry(key, dim)); +} + /* ************************************************************************* */ FastVector::iterator Scatter::find(Key key) { iterator it = begin(); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 39bfa6b8d..793961c59 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -50,10 +50,16 @@ struct GTSAM_EXPORT SlotEntry { */ class Scatter : public FastVector { public: - /// Constructor + /// Default Constructor + Scatter() {} + + /// Construct from gaussian factor graph, with optional (partial or complete) ordering Scatter(const GaussianFactorGraph& gfg, boost::optional ordering = boost::none); + /// Add a key/dim pair + void add(Key key, size_t dim); + private: /// Find the SlotEntry with the right key (linear time worst case) diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index ebe5bcc9b..d51f365be 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -32,7 +32,7 @@ #include #include #include -#include +#include #include #include @@ -59,7 +59,7 @@ namespace gtsam { /* ************************************************************************* */ static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + for(const GaussianFactor::shared_ptr &gf: gfg) { JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); if( !jf ) { jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) @@ -122,7 +122,7 @@ vector uniqueSampler(const vector &weight, const size_t n) { /* sampling and cache results */ vector samples = iidSampler(localWeights, n-count); - BOOST_FOREACH ( const size_t &id, samples ) { + for ( const size_t &id: samples ) { if ( touched[id] == false ) { touched[id] = true ; result.push_back(id); @@ -136,7 +136,7 @@ vector uniqueSampler(const vector &weight, const size_t n) { /****************************************************************************/ Subgraph::Subgraph(const std::vector &indices) { edges_.reserve(indices.size()); - BOOST_FOREACH ( const size_t &idx, indices ) { + for ( const size_t &idx: indices ) { edges_.push_back(SubgraphEdge(idx, 1.0)); } } @@ -144,7 +144,7 @@ Subgraph::Subgraph(const std::vector &indices) { /****************************************************************************/ std::vector Subgraph::edgeIndices() const { std::vector eid; eid.reserve(size()); - BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) { + for ( const SubgraphEdge &edge: edges_ ) { eid.push_back(edge.index_); } return eid; @@ -180,7 +180,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { /****************************************************************************/ std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { os << "Subgraph" << endl; - BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) { + for ( const SubgraphEdge &e: subgraph.edges() ) { os << e << ", " ; } return os; @@ -286,7 +286,7 @@ std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { std::vector result ; size_t idx = 0; - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + for ( const GaussianFactor::shared_ptr &gf: gfg ) { if ( gf->size() == 1 ) { result.push_back(idx); } @@ -299,7 +299,7 @@ std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { std::vector result ; size_t idx = 0; - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + for ( const GaussianFactor::shared_ptr &gf: gfg ) { if ( gf->size() == 2 ) { const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; if ( (k1-k0) == 1 || (k0-k1) == 1 ) @@ -332,9 +332,9 @@ std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { /* traversal */ while ( !q.empty() ) { const size_t head = q.front(); q.pop(); - BOOST_FOREACH ( const size_t id, variableIndex[head] ) { + for ( const size_t id: variableIndex[head] ) { const GaussianFactor &gf = *gfg[id]; - BOOST_FOREACH ( const size_t key, gf.keys() ) { + for ( const size_t key: gf.keys() ) { if ( flags.count(key) == 0 ) { q.push(key); flags.insert(key); @@ -360,7 +360,7 @@ std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con DSFVector D(n) ; size_t count = 0 ; double sum = 0.0 ; - BOOST_FOREACH (const size_t id, idx) { + for (const size_t id: idx) { const GaussianFactor &gf = *gfg[id]; if ( gf.keys().size() != 2 ) continue; const size_t u = ordering.find(gf.keys()[0])->second, @@ -399,7 +399,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg } /* down weight the tree edges to zero */ - BOOST_FOREACH ( const size_t id, tree ) { + for ( const size_t id: tree ) { w[id] = 0.0; } @@ -419,7 +419,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg const size_t m = gfg.size() ; Weights weight; weight.reserve(m); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) { + for(const GaussianFactor::shared_ptr &gf: gfg ) { switch ( parameters_.skeletonWeight_ ) { case SubgraphBuilderParameters::EQUAL: weight.push_back(1.0); @@ -569,7 +569,7 @@ void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const std::copy(y.data(), y.data() + y.rows(), x.data()); /* in place back substitute */ - BOOST_REVERSE_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + for (auto cg: boost::adaptors::reverse(*Rc1_)) { /* collect a subvector of x that consists of the parents of cg (S) */ const Vector xParent = getSubvector(x, keyInfo_, FastVector(cg->beginParents(), cg->endParents())); const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); @@ -589,7 +589,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const std::copy(y.data(), y.data() + y.rows(), x.data()); /* in place back substitute */ - BOOST_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + for(const boost::shared_ptr & cg: *Rc1_) { const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); // const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); const Vector solFrontal = cg->get_R().transpose().triangularView().solve(rhsFrontal); @@ -634,7 +634,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector< /* figure out dimension by traversing the keys */ size_t d = 0; - BOOST_FOREACH ( const Key &key, keys ) { + for ( const Key &key: keys ) { const KeyInfoEntry &entry = keyInfo.find(key)->second; cache.push_back(make_pair(entry.colstart(), entry.dim())); d += entry.dim(); @@ -643,7 +643,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector< /* use the cache to fill the result */ Vector result = Vector::Zero(d, 1); size_t idx = 0; - BOOST_FOREACH ( const Cache::value_type &p, cache ) { + for ( const Cache::value_type &p: cache ) { result.segment(idx, p.second) = src.segment(p.first, p.second) ; idx += p.second; } @@ -655,7 +655,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector< void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys, Vector &dst) { /* use the cache */ size_t idx = 0; - BOOST_FOREACH ( const Key &key, keys ) { + for ( const Key &key: keys ) { const KeyInfoEntry &entry = keyInfo.find(key)->second; dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; idx += entry.dim(); @@ -668,7 +668,7 @@ buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, co GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); result->reserve(subgraph.size()); - BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) { + for ( const SubgraphEdge &e: subgraph ) { const size_t idx = e.index(); if ( clone ) result->push_back(gfg[idx]->clone()); else result->push_back(gfg[idx]); diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 3c7256c29..22d39a7f2 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -131,7 +131,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); size_t t = 0; - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { + for ( const GaussianFactor::shared_ptr &gf: jfg ) { if (gf->keys().size() > 2) { throw runtime_error( @@ -144,12 +144,12 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { if (gf->keys().size() == 1) augment = true; else { - const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), - v_root = D.findSet(v); + const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.find(u), + v_root = D.find(v); if (u_root != v_root) { t++; augment = true; - D.makeUnionInPlace(u_root, v_root); + D.merge(u_root, v_root); } } if (augment) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 1439d4b61..35afddb3a 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -18,7 +18,6 @@ #include -#include #include #include #include @@ -48,20 +47,29 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Dims& dims) { typedef pair Pair; size_t j = 0; - BOOST_FOREACH(const Pair& v, dims) { + for (const Pair& v : dims) { Key key; size_t n; boost::tie(key, n) = v; - values_.insert(make_pair(key, x.segment(j,n))); + values_.insert(make_pair(key, x.segment(j, n))); j += n; } } + /* ************************************************************************* */ + VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { + size_t j = 0; + for (const SlotEntry& v : scatter) { + values_.insert(make_pair(v.key, x.segment(j, v.dimension))); + j += v.dimension; + } + } + /* ************************************************************************* */ VectorValues VectorValues::Zero(const VectorValues& other) { VectorValues result; - BOOST_FOREACH(const KeyValuePair& v, other) + for(const KeyValuePair& v: other) result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); return result; } @@ -82,7 +90,7 @@ namespace gtsam { void VectorValues::update(const VectorValues& values) { iterator hint = begin(); - BOOST_FOREACH(const KeyValuePair& key_value, values) + for(const KeyValuePair& key_value: values) { // Use this trick to find the value using a hint, since we are inserting from another sorted map size_t oldSize = values_.size(); @@ -108,14 +116,14 @@ namespace gtsam { /* ************************************************************************* */ void VectorValues::setZero() { - BOOST_FOREACH(Vector& v, values_ | map_values) + for(Vector& v: values_ | map_values) v.setZero(); } /* ************************************************************************* */ void VectorValues::print(const string& str, const KeyFormatter& formatter) const { cout << str << ": " << size() << " elements\n"; - BOOST_FOREACH(const value_type& key_value, *this) + for(const value_type& key_value: *this) cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; cout.flush(); } @@ -125,7 +133,7 @@ namespace gtsam { if(this->size() != x.size()) return false; typedef boost::tuple ValuePair; - BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { + for(const ValuePair& values: boost::combine(*this, x)) { if(values.get<0>().first != values.get<1>().first || !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) return false; @@ -138,13 +146,13 @@ namespace gtsam { { // Count dimensions DenseIndex totalDim = 0; - BOOST_FOREACH(const Vector& v, *this | map_values) + for(const Vector& v: *this | map_values) totalDim += v.size(); // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - BOOST_FOREACH(const Vector& v, *this | map_values) { + for(const Vector& v: *this | map_values) { result.segment(pos, v.size()) = v; pos += v.size(); } @@ -166,7 +174,7 @@ namespace gtsam { // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - BOOST_FOREACH(const Vector *v, items) { + for(const Vector *v: items) { result.segment(pos, v->size()) = *v; pos += v->size(); } @@ -179,11 +187,11 @@ namespace gtsam { { // Count dimensions DenseIndex totalDim = 0; - BOOST_FOREACH(size_t dim, keys | map_values) + for(size_t dim: keys | map_values) totalDim += dim; Vector result(totalDim); size_t j = 0; - BOOST_FOREACH(const Dims::value_type& it, keys) { + for(const Dims::value_type& it: keys) { result.segment(j,it.second) = at(it.first); j += it.second; } @@ -221,7 +229,7 @@ namespace gtsam { double result = 0.0; typedef boost::tuple ValuePair; using boost::adaptors::map_values; - BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) { + for(const ValuePair& values: boost::combine(*this, v)) { assert_throw(values.get<0>().first == values.get<1>().first, invalid_argument("VectorValues::dot called with a VectorValues of different structure")); assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), @@ -240,7 +248,7 @@ namespace gtsam { double VectorValues::squaredNorm() const { double sumSquares = 0.0; using boost::adaptors::map_values; - BOOST_FOREACH(const Vector& v, *this | map_values) + for(const Vector& v: *this | map_values) sumSquares += v.squaredNorm(); return sumSquares; } @@ -329,7 +337,7 @@ namespace gtsam { VectorValues operator*(const double a, const VectorValues &v) { VectorValues result; - BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v) + for(const VectorValues::KeyValuePair& key_v: v) result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); return result; } @@ -343,7 +351,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues& VectorValues::operator*=(double alpha) { - BOOST_FOREACH(Vector& v, *this | map_values) + for(Vector& v: *this | map_values) v *= alpha; return *this; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 0c752728a..cb1e08f2d 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -17,11 +17,12 @@ #pragma once +#include +#include #include #include #include #include -#include #include @@ -124,9 +125,12 @@ namespace gtsam { template VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {} - /** Constructor from Vector. */ + /// Constructor from Vector, with Dims VectorValues(const Vector& c, const Dims& dims); + /// Constructor from Vector, with Scatter + VectorValues(const Vector& c, const Scatter& scatter); + /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ static VectorValues Zero(const VectorValues& other); diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 4722a9b6d..ac8c2d7c7 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -55,10 +55,10 @@ namespace gtsam OptimizeData myData; myData.parentData = parentData; // Take any ancestor results we'll need - BOOST_FOREACH(Key parent, clique->conditional_->parents()) + for(Key parent: clique->conditional_->parents()) myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent))); + // Solve and store in our results - //collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/)); { GaussianConditional& c = *clique->conditional(); // Solve matrix @@ -68,7 +68,7 @@ namespace gtsam DenseIndex dim = 0; FastVector parentPointers; parentPointers.reserve(clique->conditional()->nrParents()); - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + for(Key parent: clique->conditional()->parents()) { parentPointers.push_back(myData.cliqueResults.at(parent)); dim += parentPointers.back()->second.size(); } @@ -76,23 +76,30 @@ namespace gtsam // Fill parent vector xS.resize(dim); DenseIndex vectorPos = 0; - BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { + for(const VectorValues::const_iterator& parentPointer: parentPointers) { const Vector& parentVector = parentPointer->second; xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); vectorPos += parentVector.size(); } } - xS = c.getb() - c.get_S() * xS; - Vector soln = c.get_R().triangularView().solve(xS); + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + + // TODO(gareth): Inline instantiation of Eigen::Solve and check flag + const Vector solution = c.get_R().triangularView().solve(rhs); // Check for indeterminant solution - if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); + if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); // Insert solution into a VectorValues DenseIndex vectorPosition = 0; for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { VectorValues::const_iterator r = - collectedResult.insert(*frontal, soln.segment(vectorPosition, c.getDim(frontal))); + collectedResult.insert(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); myData.cliqueResults.insert(make_pair(r->first, r)); vectorPosition += c.getDim(frontal); } @@ -108,7 +115,7 @@ namespace gtsam // OptimizeData myData; // myData.parentData = parentData; // // Take any ancestor results we'll need - // BOOST_FOREACH(Key parent, clique->conditional_->parents()) + // for(Key parent: clique->conditional_->parents()) // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]); // // Solve and store in our results // myData.results.insert(clique->conditional()->solve(myData.ancestorResults)); diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 26f975296..89b44f1f8 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -30,24 +30,29 @@ using namespace boost::assign; #include #include #include -#include using namespace std; using namespace gtsam; static const Key _x_=0, _y_=1; -static GaussianBayesNet smallBayesNet = list_of - (GaussianConditional(_x_, (Vector(1) << 9.0).finished(), (Matrix(1, 1) << 1.0).finished(), _y_, (Matrix(1, 1) << 1.0).finished())) - (GaussianConditional(_y_, (Vector(1) << 5.0).finished(), (Matrix(1, 1) << 1.0).finished())); +static GaussianBayesNet smallBayesNet = + list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))( + GaussianConditional(_y_, Vector1::Constant(5), I_1x1)); + +static GaussianBayesNet noisyBayesNet = + list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1, + noiseModel::Diagonal::Sigmas(Vector1::Constant(2))))( + GaussianConditional(_y_, Vector1::Constant(5), I_1x1, + noiseModel::Diagonal::Sigmas(Vector1::Constant(3)))); /* ************************************************************************* */ -TEST( GaussianBayesNet, matrix ) +TEST( GaussianBayesNet, Matrix ) { Matrix R; Vector d; boost::tie(R,d) = smallBayesNet.matrix(); // find matrix and RHS - Matrix R1 = (Matrix(2, 2) << + Matrix R1 = (Matrix2() << 1.0, 1.0, 0.0, 1.0 ).finished(); @@ -58,31 +63,55 @@ TEST( GaussianBayesNet, matrix ) } /* ************************************************************************* */ -TEST( GaussianBayesNet, optimize ) +TEST( GaussianBayesNet, NoisyMatrix ) { + Matrix R; Vector d; + boost::tie(R,d) = noisyBayesNet.matrix(); // find matrix and RHS + + Matrix R1 = (Matrix2() << + 0.5, 0.5, + 0.0, 1./3. + ).finished(); + Vector d1 = Vector2(9./2., 5./3.); + + EXPECT(assert_equal(R,R1)); + EXPECT(assert_equal(d,d1)); +} + +/* ************************************************************************* */ +TEST(GaussianBayesNet, Optimize) { + VectorValues expected = + map_list_of(_x_, Vector1::Constant(4))(_y_, Vector1::Constant(5)); VectorValues actual = smallBayesNet.optimize(); + EXPECT(assert_equal(expected, actual)); +} - VectorValues expected = map_list_of - (_x_, (Vector(1) << 4.0).finished()) - (_y_, (Vector(1) << 5.0).finished()); +/* ************************************************************************* */ +TEST(GaussianBayesNet, NoisyOptimize) { + Matrix R; + Vector d; + boost::tie(R, d) = noisyBayesNet.matrix(); // find matrix and RHS + const Vector x = R.inverse() * d; + VectorValues expected = map_list_of(_x_, x.head(1))(_y_, x.tail(1)); - EXPECT(assert_equal(expected,actual)); + VectorValues actual = noisyBayesNet.optimize(); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST( GaussianBayesNet, optimizeIncomplete ) { static GaussianBayesNet incompleteBayesNet = list_of - (GaussianConditional(_x_, (Vector(1) << 9.0).finished(), (Matrix(1, 1) << 1.0).finished(), _y_, (Matrix(1, 1) << 1.0).finished())); + (GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1)); VectorValues solutionForMissing = map_list_of - (_y_, (Vector(1) << 5.0).finished()); + (_y_, Vector1::Constant(5)); VectorValues actual = incompleteBayesNet.optimize(solutionForMissing); VectorValues expected = map_list_of - (_x_, (Vector(1) << 4.0).finished()) - (_y_, (Vector(1) << 5.0).finished()); + (_x_, Vector1::Constant(4)) + (_y_, Vector1::Constant(5)); EXPECT(assert_equal(expected,actual)); } @@ -96,13 +125,13 @@ TEST( GaussianBayesNet, optimize3 ) // NOTE: we are supplying a new RHS here VectorValues expected = map_list_of - (_x_, (Vector(1) << -1.0).finished()) - (_y_, (Vector(1) << 5.0).finished()); + (_x_, Vector1::Constant(-1)) + (_y_, Vector1::Constant(5)); // Test different RHS version VectorValues gx = map_list_of - (_x_, (Vector(1) << 4.0).finished()) - (_y_, (Vector(1) << 5.0).finished()); + (_x_, Vector1::Constant(4)) + (_y_, Vector1::Constant(5)); VectorValues actual = smallBayesNet.backSubstitute(gx); EXPECT(assert_equal(expected, actual)); } @@ -115,11 +144,11 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) // 5 1 1 3 VectorValues x = map_list_of - (_x_, (Vector(1) << 2.0).finished()) - (_y_, (Vector(1) << 5.0).finished()), + (_x_, Vector1::Constant(2)) + (_y_, Vector1::Constant(5)), expected = map_list_of - (_x_, (Vector(1) << 2.0).finished()) - (_y_, (Vector(1) << 3.0).finished()); + (_x_, Vector1::Constant(2)) + (_y_, Vector1::Constant(3)); VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); @@ -131,15 +160,15 @@ TEST( GaussianBayesNet, DeterminantTest ) { GaussianBayesNet cbn; cbn += GaussianConditional( - 0, Vector2(3.0, 4.0 ), (Matrix(2, 2) << 1.0, 3.0, 0.0, 4.0 ).finished(), - 1, (Matrix(2, 2) << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); + 0, Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(), + 1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 1, Vector2(5.0, 6.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 3.0 ).finished(), - 2, (Matrix(2, 2) << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); + 1, Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(), + 2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 3, Vector2(7.0, 8.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 5.0 ).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); + 3, Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); double expectedDeterminant = 60.0 / 64.0; double actualDeterminant = cbn.determinant(); @@ -162,21 +191,21 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), - 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), - 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); + 0, Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(), + 3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(), + 4, (Matrix2() << 11.0,12.0,13.0,14.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), - 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), - 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); + 1, Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(), + 2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(), + 4, (Matrix2() << 25.0,26.0,27.0,28.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), - 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); + 2, Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(), + 3, (Matrix2() << 35.0,36.0,37.0,38.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), - 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); + 3, Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(), + 4, (Matrix2() << 45.0,46.0,47.0,48.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); + 4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished())); // Compute the Hessian numerically Matrix hessian = numericalHessian( diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 738a6d59f..e5634c357 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -130,7 +130,7 @@ TEST(GaussianBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree GaussianBayesTree bt; bt.insertRoot( - MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) + MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()), 2, Vector3(0.2638, 0.1455, 0.1361)), list_of (MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0).finished()) diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 8f936e4b2..45f652d05 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -27,7 +27,7 @@ #include #include -#include // for operator += +#include // for operator += using namespace boost::assign; #include @@ -36,7 +36,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -//static SharedDiagonal +// static SharedDiagonal // sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), // constraintModel = noiseModel::Constrained::All(2); @@ -56,11 +56,11 @@ TEST(GaussianFactorGraph, initialization) { // Test sparse, which takes a vector and returns a matrix, used in MATLAB // Note that this the augmented vector and the RHS is in column 7 - Matrix expectedIJS = (Matrix(3, 22) << - 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8., - 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7., - 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5 - ).finished(); + Matrix expectedIJS = + (Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., + 8., 7., 8., 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., + 7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., + -5., 5., 5., -1., 1.5).finished(); Matrix actualIJS = fg.sparseJacobian_(); EQUALITY(expectedIJS, actualIJS); } @@ -98,7 +98,8 @@ TEST(GaussianFactorGraph, sparseJacobian) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model); - gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.).finished(), 1, (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13.,16.), model); + gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1, + (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model); Matrix actual = gfg.sparseJacobian_(); @@ -121,73 +122,73 @@ TEST(GaussianFactorGraph, matrices) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); gfg.add(0, A00, Vector2(4., 8.), model); - gfg.add(0, A10, 1, A11, Vector2(13.,16.), model); + gfg.add(0, A10, 1, A11, Vector2(13., 16.), model); - Matrix Ab(4,6); - Ab << - 1, 2, 3, 0, 0, 4, - 5, 6, 7, 0, 0, 8, - 9,10, 0,11,12,13, - 0, 0, 0,14,15,16; + Matrix Ab(4, 6); + Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16; // augmented versions EXPECT(assert_equal(Ab, gfg.augmentedJacobian())); EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian())); // jacobian - Matrix A = Ab.leftCols(Ab.cols()-1); - Vector b = Ab.col(Ab.cols()-1); - Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); + Matrix A = Ab.leftCols(Ab.cols() - 1); + Vector b = Ab.col(Ab.cols() - 1); + Matrix actualA; + Vector actualb; + boost::tie(actualA, actualb) = gfg.jacobian(); EXPECT(assert_equal(A, actualA)); EXPECT(assert_equal(b, actualb)); // hessian Matrix L = A.transpose() * A; Vector eta = A.transpose() * b; - Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); + Matrix actualL; + Vector actualeta; + boost::tie(actualL, actualeta) = gfg.hessian(); EXPECT(assert_equal(L, actualL)); EXPECT(assert_equal(eta, actualeta)); // hessianBlockDiagonal - VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns - expectLdiagonal.insert(0, Vector3(1+25+81, 4+36+100, 9+49)); - expectLdiagonal.insert(1, Vector2(121+196, 144+225)); + VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns + expectLdiagonal.insert(0, Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49)); + expectLdiagonal.insert(1, Vector2(121 + 196, 144 + 225)); EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal())); // hessianBlockDiagonal - map actualBD = gfg.hessianBlockDiagonal(); - LONGS_EQUAL(2,actualBD.size()); - EXPECT(assert_equal(A00.transpose()*A00 + A10.transpose()*A10,actualBD[0])); - EXPECT(assert_equal(A11.transpose()*A11,actualBD[1])); + map actualBD = gfg.hessianBlockDiagonal(); + LONGS_EQUAL(2, actualBD.size()); + EXPECT(assert_equal(A00.transpose() * A00 + A10.transpose() * A10, actualBD[0])); + EXPECT(assert_equal(A11.transpose() * A11, actualBD[1])); } /* ************************************************************************* */ +/// Factor graph with 2 2D factors on 3 2D variables static GaussianFactorGraph createSimpleGaussianFactorGraph() { GaussianFactorGraph fg; + Key x1 = 2, x2 = 0, l1 = 1; SharedDiagonal unit2 = noiseModel::Unit::Create(2); // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(2, 10*I_2x2, -1.0*Vector::Ones(2), unit2); + fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(0, 10*I_2x2, 2, -10*I_2x2, Vector2(2.0, -1.0), unit2); + fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(1, 5*I_2x2, 2, -5*I_2x2, Vector2(0.0, 1.0), unit2); + fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*I_2x2, 1, 5*I_2x2, Vector2(-1.0, 1.5), unit2); + fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); return fg; } /* ************************************************************************* */ -TEST( GaussianFactorGraph, gradient ) -{ +TEST(GaussianFactorGraph, gradient) { GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); // Construct expected gradient - // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 + // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + + // 25*(l1-x2-[-0.2;0.3])^2 // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] - VectorValues expected = map_list_of - (1, Vector2(5.0, -12.5)) - (2, Vector2(30.0, 5.0)) - (0, Vector2(-25.0, 17.5)); + VectorValues expected = map_list_of(1, Vector2(5.0, -12.5))(2, Vector2(30.0, 5.0))( + 0, Vector2(-25.0, 17.5)); // Check the gradient at delta=0 VectorValues zero = VectorValues::Zero(expected); @@ -202,18 +203,14 @@ TEST( GaussianFactorGraph, gradient ) } /* ************************************************************************* */ -TEST( GaussianFactorGraph, transposeMultiplication ) -{ +TEST(GaussianFactorGraph, transposeMultiplication) { GaussianFactorGraph A = createSimpleGaussianFactorGraph(); - Errors e; e += - Vector2(0.0, 0.0), - Vector2(15.0, 0.0), - Vector2(0.0,-5.0), - Vector2(-7.5,-5.0); + Errors e; + e += Vector2(0.0, 0.0), Vector2(15.0, 0.0), Vector2(0.0, -5.0), Vector2(-7.5, -5.0); VectorValues expected; - expected.insert(1, Vector2(-37.5,-50.0)); + expected.insert(1, Vector2(-37.5, -50.0)); expected.insert(2, Vector2(-150.0, 25.0)); expected.insert(0, Vector2(187.5, 25.0)); @@ -222,8 +219,7 @@ TEST( GaussianFactorGraph, transposeMultiplication ) } /* ************************************************************************* */ -TEST(GaussianFactorGraph, eliminate_empty ) -{ +TEST(GaussianFactorGraph, eliminate_empty) { // eliminate an empty factor GaussianFactorGraph gfg; gfg.add(JacobianFactor()); @@ -243,25 +239,31 @@ TEST(GaussianFactorGraph, eliminate_empty ) } /* ************************************************************************* */ -TEST( GaussianFactorGraph, matrices2 ) -{ +TEST(GaussianFactorGraph, matrices2) { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); - Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); - EXPECT(assert_equal(A.transpose()*A, AtA)); - EXPECT(assert_equal(A.transpose()*b, eta)); + Matrix A; + Vector b; + boost::tie(A, b) = gfg.jacobian(); + Matrix AtA; + Vector eta; + boost::tie(AtA, eta) = gfg.hessian(); + EXPECT(assert_equal(A.transpose() * A, AtA)); + EXPECT(assert_equal(A.transpose() * b, eta)); + Matrix expectedAtA(6, 6); + expectedAtA << 125, 0, -25, 0, -100, 0, // + 0, 125, 0, -25, 0, -100, // + -25, 0, 50, 0, -25, 0, // + 0, -25, 0, 50, 0, -25, // + -100, 0, -25, 0, 225, 0, // + 0, -100, 0, -25, 0, 225; + EXPECT(assert_equal(expectedAtA, AtA)); } - /* ************************************************************************* */ -TEST( GaussianFactorGraph, multiplyHessianAdd ) -{ +TEST(GaussianFactorGraph, multiplyHessianAdd) { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - VectorValues x = map_list_of - (0, Vector2(1,2)) - (1, Vector2(3,4)) - (2, Vector2(5,6)); + VectorValues x = map_list_of(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6)); VectorValues expected; expected.insert(0, Vector2(-450, -450)); @@ -274,7 +276,7 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) // now, do it with non-zero y gfg.multiplyHessianAdd(1.0, x, actual); - EXPECT(assert_equal(2*expected, actual)); + EXPECT(assert_equal(2 * expected, actual)); } /* ************************************************************************* */ @@ -286,20 +288,20 @@ static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { } /* ************************************************************************* */ -TEST( GaussianFactorGraph, multiplyHessianAdd2 ) -{ +TEST(GaussianFactorGraph, multiplyHessianAdd2) { GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); // brute force - Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); - Vector X(6); X<<1,2,3,4,5,6; - Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; - EXPECT(assert_equal(Y,AtA*X)); + Matrix AtA; + Vector eta; + boost::tie(AtA, eta) = gfg.hessian(); + Vector X(6); + X << 1, 2, 3, 4, 5, 6; + Vector Y(6); + Y << -450, -450, 300, 400, 2950, 3450; + EXPECT(assert_equal(Y, AtA * X)); - VectorValues x = map_list_of - (0, Vector2(1,2)) - (1, Vector2(3,4)) - (2, Vector2(5,6)); + VectorValues x = map_list_of(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6)); VectorValues expected; expected.insert(0, Vector2(-450, -450)); @@ -312,24 +314,26 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) // now, do it with non-zero y gfg.multiplyHessianAdd(1.0, x, actual); - EXPECT(assert_equal(2*expected, actual)); + EXPECT(assert_equal(2 * expected, actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, matricesMixed ) -{ +TEST(GaussianFactorGraph, matricesMixed) { GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); - Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); // incorrect ! - Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); // correct - EXPECT(assert_equal(A.transpose()*A, AtA)); - Vector expected = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished(); + Matrix A; + Vector b; + boost::tie(A, b) = gfg.jacobian(); // incorrect ! + Matrix AtA; + Vector eta; + boost::tie(AtA, eta) = gfg.hessian(); // correct + EXPECT(assert_equal(A.transpose() * A, AtA)); + Vector expected = -(Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished(); EXPECT(assert_equal(expected, eta)); - EXPECT(assert_equal(A.transpose()*b, eta)); + EXPECT(assert_equal(A.transpose() * b, eta)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, gradientAtZero ) -{ +TEST(GaussianFactorGraph, gradientAtZero) { GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); VectorValues expected; VectorValues actual = gfg.gradientAtZero(); @@ -340,29 +344,28 @@ TEST( GaussianFactorGraph, gradientAtZero ) } /* ************************************************************************* */ -TEST( GaussianFactorGraph, clone ) { +TEST(GaussianFactorGraph, clone) { // 2 variables, frontal has dim=4 VerticalBlockMatrix blockMatrix(list_of(4)(2)(1), 4); - blockMatrix.matrix() << - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4; + blockMatrix.matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0, + 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4; GaussianConditional cg(list_of(1)(2), 1, blockMatrix); GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor(); - init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor + init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor init_graph.push_back(GaussianConditional(cg)); - GaussianFactorGraph exp_graph = createGaussianFactorGraphWithHessianFactor(); // Created separately - exp_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor + GaussianFactorGraph exp_graph = + createGaussianFactorGraphWithHessianFactor(); // Created separately + exp_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor exp_graph.push_back(GaussianConditional(cg)); GaussianFactorGraph actCloned = init_graph.clone(); - EXPECT(assert_equal(init_graph, actCloned)); // Same as the original version + EXPECT(assert_equal(init_graph, actCloned)); // Same as the original version // Apply an in-place change to init_graph and compare - JacobianFactor::shared_ptr jacFactor0 = boost::dynamic_pointer_cast(init_graph.at(0)); + JacobianFactor::shared_ptr jacFactor0 = + boost::dynamic_pointer_cast(init_graph.at(0)); CHECK(jacFactor0); jacFactor0->getA(jacFactor0->begin()) *= 7.; EXPECT(assert_inequal(init_graph, exp_graph)); @@ -370,9 +373,9 @@ TEST( GaussianFactorGraph, clone ) { } /* ************************************************************************* */ -TEST( GaussianFactorGraph, negate ) { +TEST(GaussianFactorGraph, negate) { GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor(); - init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor + init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor GaussianFactorGraph actNegation = init_graph.negate(); GaussianFactorGraph expNegation; expNegation.push_back(init_graph.at(0)->negate()); @@ -385,8 +388,7 @@ TEST( GaussianFactorGraph, negate ) { } /* ************************************************************************* */ -TEST( GaussianFactorGraph, hessianDiagonal ) -{ +TEST(GaussianFactorGraph, hessianDiagonal) { GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); VectorValues expected; Matrix infoMatrix = gfg.hessian().first; @@ -399,6 +401,16 @@ TEST( GaussianFactorGraph, hessianDiagonal ) EXPECT(assert_equal(expected, actual)); } +TEST(GaussianFactorGraph, DenseSolve) { + GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); + VectorValues expected = fg.optimize(); + VectorValues actual = fg.optimizeDensely(); + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 911100337..61fa8bd2c 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -53,8 +53,8 @@ TEST(HessianFactor, emptyConstructor) { HessianFactor f; DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero - EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty - EXPECT(assert_equal((Matrix) Z_1x1, f.info())); // Full matrix should be 1-by-1 zero matrix + //EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty + EXPECT(assert_equal((Matrix) Z_1x1, f.info().selfadjointView())); // Full matrix should be 1-by-1 zero matrix DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error } @@ -103,7 +103,7 @@ TEST(HessianFactor, Constructor1) HessianFactor factor(0, G, g, f); // extract underlying parts - EXPECT(assert_equal(G, Matrix(factor.info(factor.begin(), factor.begin())))); + EXPECT(assert_equal(G, Matrix(factor.info().diagonalBlock(0)))); EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); EXPECT(assert_equal(g, Vector(factor.linearTerm()))); EXPECT_LONGS_EQUAL(1, (long)factor.size()); @@ -118,7 +118,6 @@ TEST(HessianFactor, Constructor1) EXPECT_DOUBLES_EQUAL(expected, actual, 1e-10); } - /* ************************************************************************* */ TEST(HessianFactor, Constructor1b) { @@ -132,7 +131,7 @@ TEST(HessianFactor, Constructor1b) double f = dot(g,mu); // Check - EXPECT(assert_equal(G, Matrix(factor.info(factor.begin(), factor.begin())))); + EXPECT(assert_equal(G, Matrix(factor.info().diagonalBlock(0)))); EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); EXPECT(assert_equal(g, Vector(factor.linearTerm()))); EXPECT_LONGS_EQUAL(1, (long)factor.size()); @@ -167,9 +166,9 @@ TEST(HessianFactor, Constructor2) Vector linearExpected(3); linearExpected << g1, g2; EXPECT(assert_equal(linearExpected, factor.linearTerm())); - EXPECT(assert_equal(G11, factor.info(factor.begin(), factor.begin()))); - EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1))); - EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); + EXPECT(assert_equal(G11, factor.info().diagonalBlock(0))); + EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1))); + EXPECT(assert_equal(G22, factor.info().diagonalBlock(1))); // Check case when vector values is larger than factor VectorValues dxLarge = pair_list_of @@ -218,12 +217,12 @@ TEST(HessianFactor, Constructor3) Vector linearExpected(6); linearExpected << g1, g2, g3; EXPECT(assert_equal(linearExpected, factor.linearTerm())); - EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0))); - EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1))); - EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2))); - EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); - EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2))); - EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2))); + EXPECT(assert_equal(G11, factor.info().diagonalBlock(0))); + EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1))); + EXPECT(assert_equal(G13, factor.info().aboveDiagonalBlock(0, 2))); + EXPECT(assert_equal(G22, factor.info().diagonalBlock(1))); + EXPECT(assert_equal(G23, factor.info().aboveDiagonalBlock(1, 2))); + EXPECT(assert_equal(G33, factor.info().diagonalBlock(2))); } /* ************************************************************************* */ @@ -271,12 +270,12 @@ TEST(HessianFactor, ConstructorNWay) Vector linearExpected(6); linearExpected << g1, g2, g3; EXPECT(assert_equal(linearExpected, factor.linearTerm())); - EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0))); - EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1))); - EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2))); - EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); - EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2))); - EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2))); + EXPECT(assert_equal(G11, factor.info().diagonalBlock(0))); + EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1))); + EXPECT(assert_equal(G13, factor.info().aboveDiagonalBlock(0, 2))); + EXPECT(assert_equal(G22, factor.info().diagonalBlock(1))); + EXPECT(assert_equal(G23, factor.info().aboveDiagonalBlock(1, 2))); + EXPECT(assert_equal(G33, factor.info().diagonalBlock(2))); } /* ************************************************************************* */ @@ -499,7 +498,7 @@ TEST(HessianFactor, combine) { -100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000, 0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000, 25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500).finished(); - EXPECT(assert_equal(expected, Matrix(actual.matrixObject().full()), tol)); + EXPECT(assert_equal(expected, Matrix(actual.info().selfadjointView()), tol)); } @@ -575,6 +574,23 @@ TEST(HessianFactor, hessianDiagonal) EXPECT(assert_equal(G22,actualBD[1])); } +/* ************************************************************************* */ +TEST(HessianFactor, Solve) +{ + Matrix2 A; + A << 1, 2, 3, 4; + Matrix2 G = A.transpose() * A; + Vector2 b(5, 6); + Vector2 g = A.transpose() * b; + double f = 0; + Key key(55); + HessianFactor factor(key, G, g, f); + + VectorValues expected; + expected.insert(key, A.inverse() * b); + EXPECT(assert_equal(expected, factor.solve())); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 43ce271f3..6b130bea0 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -22,7 +22,6 @@ #include -#include #include #include @@ -58,32 +57,32 @@ TEST(NoiseModel, constructors) m.push_back(Isotropic::Precision(3, prc,false)); // test kSigmas - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(kSigmas,mi->sigmas())); // test whiten - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(whitened,mi->whiten(unwhitened))); // test unwhiten - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); // test Mahalanobis distance double distance = 5*5+10*10+15*15; - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); // test R matrix - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(R,mi->R())); // test covariance - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(kCovariance,mi->covariance())); // test covariance - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(kCovariance.inverse(),mi->information())); // test Whiten operator @@ -92,7 +91,7 @@ TEST(NoiseModel, constructors) 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0).finished()); Matrix expected = kInverseSigma * H; - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(expected,mi->Whiten(H))); // can only test inplace version once :-) @@ -449,6 +448,15 @@ TEST(NoiseModel, WhitenInPlace) } /* ************************************************************************* */ + +/* + * These tests are responsible for testing the weight functions for the m-estimators in GTSAM. + * The weight function is related to the analytic derivative of the residual function. See + * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * for details. This weight function is required when optimizing cost functions with robust + * penalties using iteratively re-weighted least squares. + */ + TEST(NoiseModel, robustFunctionHuber) { const double k = 5.0, error1 = 1.0, error2 = 10.0; @@ -479,6 +487,26 @@ TEST(NoiseModel, robustFunctionDCS) DOUBLES_EQUAL(0.00039211, weight2, 1e-8); } +TEST(NoiseModel, robustFunctionL2WithDeadZone) +{ + const double k = 1.0, e0 = -10.0, e1 = -1.01, e2 = -0.99, e3 = 0.99, e4 = 1.01, e5 = 10.0; + const mEstimator::L2WithDeadZone::shared_ptr lsdz = mEstimator::L2WithDeadZone::Create(k); + + DOUBLES_EQUAL(0.9, lsdz->weight(e0), 1e-8); + DOUBLES_EQUAL(0.00990099009, lsdz->weight(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->weight(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->weight(e3), 1e-8); + DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); + DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); + + DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8); +} + /* ************************************************************************* */ TEST(NoiseModel, robustNoiseHuber) { @@ -551,6 +579,31 @@ TEST(NoiseModel, robustNoiseDCS) DOUBLES_EQUAL(sqrt_weight*a11, A(1,1), 1e-8); } +TEST(NoiseModel, robustNoiseL2WithDeadZone) +{ + double dead_zone_size = 1.0; + SharedNoiseModel robust = noiseModel::Robust::Create( + noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size), + Unit::Create(3)); + +/* + * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes + * implement a residual function, and GTSAM calls the weight function to evaluate the + * total penalty, rather than calling the residual function. The weight function should be + * used during iteratively reweighted least squares optimization, but should not be used to + * evaluate the total penalty. The long-term solution is for all mEstimators to implement + * both a weight and a residual function, and for GTSAM to call the residual function when + * evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it + * commented out until the underlying bug in GTSAM is fixed. + * + * for (int i = 0; i < 5; i++) { + * Vector3 error = Vector3(i, 0, 0); + * DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->distance(error), 1e-8); + * } + */ + +} + /* ************************************************************************* */ #define TEST_GAUSSIAN(gaussian)\ EQUALITY(info, gaussian->information());\ diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp index f53803dd1..1618451f3 100644 --- a/gtsam/linear/tests/testRegularHessianFactor.cpp +++ b/gtsam/linear/tests/testRegularHessianFactor.cpp @@ -128,9 +128,8 @@ TEST(RegularHessianFactor, Constructors) EXPECT(assert_equal(2*expected_y, fast_y)); // check some expressions - EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal())); - EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView())); - EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal())); + EXPECT(assert_equal(G12,factor.info().aboveDiagonalBlock(i1 - factor.begin(), i2 - factor.begin()))); + EXPECT(assert_equal(G22,factor.info().diagonalBlock(i2 - factor.begin()))); } } diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 949ec3a59..7e972903a 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -188,6 +188,14 @@ TEST(VectorValues, convert) VectorValues actual(x,dims); EXPECT(assert_equal(expected, actual)); + Scatter scatter; + scatter.emplace_back(0,1); + scatter.emplace_back(1,2); + scatter.emplace_back(2,2); + scatter.emplace_back(5,2); + VectorValues actual2(x,scatter); + EXPECT(assert_equal(expected, actual2)); + // Test other direction, note vector() is not guaranteed to give right result FastVector keys = list_of(0)(1)(2)(5); EXPECT(assert_equal(x, actual.vector(keys))); diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a961a79bd..21c4200a9 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -31,22 +31,21 @@ using namespace std; //------------------------------------------------------------------------------ // Inner class PreintegratedCombinedMeasurements //------------------------------------------------------------------------------ -void PreintegratedCombinedMeasurements::print( - const string& s) const { - PreintegrationBase::print(s); +void PreintegratedCombinedMeasurements::print(const string& s) const { + PreintegrationType::print(s); cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool PreintegratedCombinedMeasurements::equals( const PreintegratedCombinedMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) && - equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); + return PreintegrationType::equals(other, tol) + && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration() { - PreintegrationBase::resetIntegration(); + PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } @@ -68,9 +67,9 @@ void PreintegratedCombinedMeasurements::resetIntegration() { void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements. - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF @@ -79,8 +78,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // and preintegrated measurements // Single Jacobians to propagate covariance - // TODO(frank): should we not also accout for bias on position? - Matrix3 theta_H_biasOmega = - C.topRows<3>(); + // TODO(frank): should we not also account for bias on position? + Matrix3 theta_H_biasOmega = -C.topRows<3>(); Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); // overall Jacobian wrt preintegrated measurements (df/dx) @@ -105,18 +104,18 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // BLOCK DIAGONAL TERMS D_t_t(&G_measCov_Gt) = dt * iCov; - D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc * - (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) * - (vel_H_biasAcc.transpose()); - D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega * - (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) * - (theta_H_biasOmega.transpose()); + D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc + * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) + * (vel_H_biasAcc.transpose()); + D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega + * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) + * (theta_H_biasOmega.transpose()); D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) * - theta_H_biasOmega.transpose(); + Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) + * theta_H_biasOmega.transpose(); D_v_R(&G_measCov_Gt) = temp; D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; @@ -131,7 +130,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration) { if (!use2ndOrderIntegration) - throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; boost::shared_ptr p = Params::MakeSharedD(); p->gyroscopeCovariance = measuredOmegaCovariance; @@ -148,12 +147,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor( - Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const PreintegratedCombinedMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias_i, bias_j), - _PIM_(pim) {} +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, + Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), _PIM_(pim) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -195,8 +194,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, Matrix93 D_r_vel_i, D_r_vel_j; // error wrt preintegrated measurements - Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, + Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, + bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); // if we need the jacobians @@ -250,11 +249,11 @@ CombinedImuFactor::CombinedImuFactor( const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias_i, bias_j), - _PIM_(pim) { +: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), +_PIM_(pim) { boost::shared_ptr p = - boost::make_shared(pim.p()); + boost::make_shared(pim.p()); p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; @@ -263,12 +262,12 @@ CombinedImuFactor::CombinedImuFactor( } void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - Pose3& pose_j, Vector3& vel_j, - const imuBias::ConstantBias& bias_i, - CombinedPreintegratedMeasurements& pim, - const Vector3& n_gravity, - const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis) { + Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { // use deprecated predict PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, omegaCoriolis, use2ndOrderCoriolis); @@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, } #endif -} /// namespace gtsam +} + /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 3141f8245..bcad9d8f7 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -22,12 +22,19 @@ #pragma once /* GTSAM includes */ +#include +#include #include -#include #include namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -57,7 +64,7 @@ namespace gtsam { * * @addtogroup SLAM */ -class PreintegratedCombinedMeasurements : public PreintegrationBase { +class PreintegratedCombinedMeasurements : public PreintegrationType { public: @@ -123,7 +130,7 @@ public: PreintegratedCombinedMeasurements( const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) - : PreintegrationBase(p, biasHat) { + : PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); } @@ -133,10 +140,10 @@ public: /// @{ /// Re-initialize PreintegratedCombinedMeasurements - void resetIntegration(); + void resetIntegration() override; /// const reference to params, shadows definition in base class - Params& p() const { return *boost::static_pointer_cast(p_);} + Params& p() const { return *boost::static_pointer_cast(this->p_);} /// @} /// @name Access instance variables @@ -146,7 +153,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s = "Preintegrated Measurements:") const; + void print(const std::string& s = "Preintegrated Measurements:") const override; bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const; /// @} @@ -163,7 +170,7 @@ public: * frame) */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT); + const Vector3& measuredOmega, const double dt) override; /// @} @@ -183,7 +190,7 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 6d36539e4..cc1dc087e 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -32,20 +32,20 @@ using namespace std; // Inner class PreintegratedImuMeasurements //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::print(const string& s) const { - PreintegrationBase::print(s); + PreintegrationType::print(s); cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl; } //------------------------------------------------------------------------------ bool PreintegratedImuMeasurements::equals( const PreintegratedImuMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) + return PreintegrationType::equals(other, tol) && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::resetIntegration() { - PreintegrationBase::resetIntegration(); + PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } @@ -53,9 +53,9 @@ void PreintegratedImuMeasurements::resetIntegration() { void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements (also get Jacobian) - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a @@ -73,31 +73,34 @@ void PreintegratedImuMeasurements::integrateMeasurement( preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 - preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt; + preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt; } //------------------------------------------------------------------------------ -void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs, - const Matrix& measuredOmegas, - const Matrix& dts) { - assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); +void PreintegratedImuMeasurements::integrateMeasurements( + const Matrix& measuredAccs, const Matrix& measuredOmegas, + const Matrix& dts) { + assert( + measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); assert(dts.cols() >= 1); assert(measuredAccs.cols() == dts.cols()); assert(measuredOmegas.cols() == dts.cols()); size_t n = static_cast(dts.cols()); for (size_t j = 0; j < n; j++) { - integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j)); + integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j)); } } //------------------------------------------------------------------------------ -void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // - Matrix9* H1, Matrix9* H2) { - PreintegrationBase::mergeWith(pim12, H1, H2); - preintMeasCov_ = - *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); +#ifdef GTSAM_TANGENT_PREINTEGRATION +void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // + Matrix9* H1, Matrix9* H2) { + PreintegrationType::mergeWith(pim12, H1, H2); + // NOTE(gareth): Temporary P is needed as of Eigen 3.3 + const Matrix9 P = *H1 * preintMeasCov_ * H1->transpose(); + preintMeasCov_ = P + *H2 * pim12.preintMeasCov_ * H2->transpose(); } - +#endif //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PreintegratedImuMeasurements::PreintegratedImuMeasurements( @@ -174,16 +177,17 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, } //------------------------------------------------------------------------------ +#ifdef GTSAM_TANGENT_PREINTEGRATION PreintegratedImuMeasurements ImuFactor::Merge( const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim12) { if (!pim01.matchesParamsWith(pim12)) - throw std::domain_error( - "Cannot merge PreintegratedImuMeasurements with different params"); + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with different params"); if (pim01.params()->body_P_sensor) - throw std::domain_error( - "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); // the bias for the merged factor will be the bias from 01 PreintegratedImuMeasurements pim02 = pim01; @@ -196,26 +200,27 @@ PreintegratedImuMeasurements ImuFactor::Merge( //------------------------------------------------------------------------------ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, - const shared_ptr& f12) { + const shared_ptr& f12) { // IMU bias keys must be the same. if (f01->key5() != f12->key5()) - throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); + throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); // expect intermediate pose, velocity keys to matchup. if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) - throw std::domain_error( - "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); + throw std::domain_error( + "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); // return new factor auto pim02 = - Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); - return boost::make_shared(f01->key1(), // P0 - f01->key2(), // V0 - f12->key3(), // P2 - f12->key4(), // V2 - f01->key5(), // B - pim02); + Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); + return boost::make_shared(f01->key1(),// P0 + f01->key2(),// V0 + f12->key3(),// P2 + f12->key4(),// V2 + f01->key5(),// B + pim02); } +#endif //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 @@ -248,9 +253,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, //------------------------------------------------------------------------------ // ImuFactor2 methods //------------------------------------------------------------------------------ -ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias), - _PIM_(pim) {} +ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, + const PreintegratedImuMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, + bias), _PIM_(pim) { +} //------------------------------------------------------------------------------ NonlinearFactor::shared_ptr ImuFactor2::clone() const { @@ -266,9 +273,11 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) { } //------------------------------------------------------------------------------ -void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) - << "," << keyFormatter(this->key3()) << ")\n"; +void ImuFactor2::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) + << ")\n"; cout << *this << endl; } @@ -281,15 +290,15 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j, - const imuBias::ConstantBias& bias_i, // - boost::optional H1, - boost::optional H2, - boost::optional H3) const { +Vector ImuFactor2::evaluateError(const NavState& state_i, + const NavState& state_j, + const imuBias::ConstantBias& bias_i, // + boost::optional H1, boost::optional H2, + boost::optional H3) const { return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); } //------------------------------------------------------------------------------ } - // namespace gtsam +// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 010550eb1..532abdac0 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -23,11 +23,18 @@ /* GTSAM includes */ #include -#include +#include +#include #include namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating @@ -61,7 +68,7 @@ namespace gtsam { * * @addtogroup SLAM */ -class PreintegratedImuMeasurements: public PreintegrationBase { +class PreintegratedImuMeasurements: public PreintegrationType { friend class ImuFactor; friend class ImuFactor2; @@ -85,29 +92,28 @@ public: */ PreintegratedImuMeasurements(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : - PreintegrationBase(p, biasHat) { + PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); } /** * Construct preintegrated directly from members: base class and preintMeasCov - * @param base PreintegrationBase instance + * @param base PreintegrationType instance * @param preintMeasCov Covariance matrix used in noise model. */ - PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov) - : PreintegrationBase(base), + PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) + : PreintegrationType(base), preintMeasCov_(preintMeasCov) { } /// print - void print(const std::string& s = "Preintegrated Measurements:") const; + void print(const std::string& s = "Preintegrated Measurements:") const override; /// equals - bool equals(const PreintegratedImuMeasurements& expected, - double tol = 1e-9) const; + bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const; /// Re-initialize PreintegratedIMUMeasurements - void resetIntegration(); + void resetIntegration() override; /** * Add a single IMU measurement to the preintegration. @@ -115,7 +121,8 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */ - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt); + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt) override; /// Add multiple measurements, in matrix columns void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas, @@ -124,8 +131,10 @@ public: /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } +#ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge in a different set of measurements and update bias derivatives accordingly void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2); +#endif #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated constructor @@ -150,7 +159,7 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); } }; @@ -230,6 +239,7 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; +#ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge two pre-integrated measurement classes static PreintegratedImuMeasurements Merge( const PreintegratedImuMeasurements& pim01, @@ -237,6 +247,7 @@ public: /// Merge two factors static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12); +#endif #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated typename diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp new file mode 100644 index 000000000..cc88d9101 --- /dev/null +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * 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 ManifoldPreintegration.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "ManifoldPreintegration.h" + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +ManifoldPreintegration::ManifoldPreintegration( + const boost::shared_ptr& p, const Bias& biasHat) : + PreintegrationBase(p, biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void ManifoldPreintegration::resetIntegration() { + deltaTij_ = 0.0; + deltaXij_ = NavState(); + delRdelBiasOmega_.setZero(); + delPdelBiasAcc_.setZero(); + delPdelBiasOmega_.setZero(); + delVdelBiasAcc_.setZero(); + delVdelBiasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && deltaXij_.equals(other.deltaXij_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void ManifoldPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { + + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // Update Jacobians + // TODO(frank): Try same simplification as in new approach + Matrix3 D_acc_R; + oldRij.rotate(acc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = omega * dt; + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; +} + +//------------------------------------------------------------------------------ +Vector9 ManifoldPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Vector9 xi; + Matrix3 D_dR_correctedRij; + // TODO(frank): could line below be simplified? It is equivalent to + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) + NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + + delPdelBiasOmega_ * biasIncr.gyroscope(); + NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; + D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; + D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; + (*H) << D_dR_bias, D_dP_bias, D_dV_bias; + } + return xi; +} + +//------------------------------------------------------------------------------ + +}// namespace gtsam diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h new file mode 100644 index 000000000..92d3f4814 --- /dev/null +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 ManifoldPreintegration.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * IMU pre-integration on NavSatet manifold. + * This corresponds to the original RSS paper (with one difference: V is rotated) + */ +class ManifoldPreintegration : public PreintegrationBase { + protected: + + /** + * Pre-integrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] + */ + NavState deltaXij_; + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + + /// Default constructor for serialization + ManifoldPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + ManifoldPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + NavState deltaXij() const override { return deltaXij_; } + Rot3 deltaRij() const override { return deltaXij_.attitude(); } + Vector3 deltaPij() const override { return deltaXij_.position(); } + Vector3 deltaVij() const override { return deltaXij_.velocity(); } + + Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; } + Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; } + Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; } + Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; } + Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; } + + /// @name Testable + /// @{ + bool equals(const ManifoldPreintegration& other, double tol) const; + /// @} + + /// @name Main functionality + /// @{ + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); + ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); + ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); + ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); + ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index f01a55577..50949c761 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -24,6 +24,18 @@ namespace gtsam { #define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; +//------------------------------------------------------------------------------ +NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v, + OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 3> H3) { + if (H1) + *H1 << I_3x3, Z_3x3, Z_3x3; + if (H2) + *H2 << Z_3x3, R.transpose(), Z_3x3; + if (H3) + *H3 << Z_3x3, Z_3x3, R.transpose(); + return NavState(R, t, v); +} //------------------------------------------------------------------------------ NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { @@ -94,138 +106,55 @@ bool NavState::equals(const NavState& other, double tol) const { } //------------------------------------------------------------------------------ -NavState NavState::inverse() const { +NavState NavState::retract(const Vector9& xi, // + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { TIE(nRb, n_t, n_v, *this); - const Rot3 bRn = nRb.inverse(); - return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); -} - -//------------------------------------------------------------------------------ -NavState NavState::operator*(const NavState& bTc) const { - TIE(nRb, n_t, n_v, *this); - TIE(bRc, b_t, b_v, bTc); - return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); -} - -//------------------------------------------------------------------------------ -NavState::PositionAndVelocity // -NavState::operator*(const PositionAndVelocity& b_tv) const { - TIE(nRb, n_t, n_v, *this); - const Point3& b_t = b_tv.first; - const Velocity3& b_v = b_tv.second; - return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v); -} - -//------------------------------------------------------------------------------ -Point3 NavState::operator*(const Point3& b_t) const { - return Point3(R_ * b_t + t_); -} - -//------------------------------------------------------------------------------ -NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, - OptionalJacobian<9, 9> H) { - Matrix3 D_R_xi; - const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0); - const Point3 p = Point3(dP(xi)); - const Vector v = dV(xi); - const NavState result(R, p, v); - if (H) { - *H << D_R_xi, Z_3x3, Z_3x3, // - Z_3x3, R.transpose(), Z_3x3, // - Z_3x3, Z_3x3, R.transpose(); + Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; + const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); + const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); + const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0); + const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0); + if (H1) { + *H1 << D_R_nRb, Z_3x3, Z_3x3, // + // Note(frank): the derivative of n_t with respect to xi is nRb + // We pre-multiply with nRc' to account for NavState::Create + // Then we make use of the identity nRc' * nRb = bRc' + nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3, + // Similar reasoning for v: + nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose(); } - return result; + if (H2) { + *H2 << D_bRc_xi, Z_3x3, Z_3x3, // + Z_3x3, bRc.transpose(), Z_3x3, // + Z_3x3, Z_3x3, bRc.transpose(); + } + return NavState(nRc, t, v); } //------------------------------------------------------------------------------ -Vector9 NavState::ChartAtOrigin::Local(const NavState& x, - OptionalJacobian<9, 9> H) { +Vector9 NavState::localCoordinates(const NavState& g, // + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + Matrix3 D_dR_R, D_dt_R, D_dv_R; + const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); + const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + Vector9 xi; Matrix3 D_xi_R; - xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v(); - if (H) { - *H << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, x.R(), Z_3x3, // - Z_3x3, Z_3x3, x.R(); + xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv; + if (H1) { + *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + D_dt_R, -I_3x3, Z_3x3, // + D_dv_R, Z_3x3, -I_3x3; + } + if (H2) { + *H2 << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, dR.matrix(), Z_3x3, // + Z_3x3, Z_3x3, dR.matrix(); } return xi; } -//------------------------------------------------------------------------------ -NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) { - if (H) - throw runtime_error("NavState::Expmap derivative not implemented yet"); - - Eigen::Block n_omega_nb = dR(xi); - Eigen::Block v = dP(xi); - Eigen::Block a = dV(xi); - - // NOTE(frank): See Pose3::Expmap - Rot3 nRb = Rot3::Expmap(n_omega_nb); - double theta2 = n_omega_nb.dot(n_omega_nb); - if (theta2 > numeric_limits::epsilon()) { - // Expmap implements a "screw" motion in the direction of n_omega_nb - Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis - Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis - Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel) - / theta2; - Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis - Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis - Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2; - return NavState(nRb, n_t, n_v); - } else { - return NavState(nRb, Point3(v), a); - } -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { - if (H) - throw runtime_error("NavState::Logmap derivative not implemented yet"); - - TIE(nRb, n_p, n_v, nTb); - Vector3 n_t = n_p; - - // NOTE(frank): See Pose3::Logmap - Vector9 xi; - Vector3 n_omega_nb = Rot3::Logmap(nRb); - double theta = n_omega_nb.norm(); - if (theta * theta <= numeric_limits::epsilon()) { - xi << n_omega_nb, n_t, n_v; - } else { - Matrix3 W = skewSymmetric(n_omega_nb / theta); - // Formula from Agrawal06iros, equation (14) - // simplified with Mathematica, and multiplying in n_t to avoid matrix math - double factor = (1 - theta / (2. * tan(0.5 * theta))); - Vector3 n_x = W * n_t; - Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x); - Vector3 n_y = W * n_v; - Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y); - xi << n_omega_nb, v, a; - } - return xi; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::AdjointMap() const { - // NOTE(frank): See Pose3::AdjointMap - const Matrix3 nRb = R(); - Matrix3 pAr = skewSymmetric(t()) * nRb; - Matrix3 vAr = skewSymmetric(v()) * nRb; - Matrix9 adj; - // nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV - adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb; - return adj; -} - -//------------------------------------------------------------------------------ -Matrix7 NavState::wedge(const Vector9& xi) { - const Matrix3 Omega = skewSymmetric(dR(xi)); - Matrix7 T; - T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0; - return T; -} - //------------------------------------------------------------------------------ // sugar for derivative blocks #define D_R_R(H) (H)->block<3,3>(0,0) @@ -239,7 +168,6 @@ Matrix7 NavState::wedge(const Vector9& xi) { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { @@ -282,7 +210,6 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, } return newState; } -#endif //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index eabd1f39b..a1544735d 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -20,7 +20,7 @@ #include #include -#include +#include namespace gtsam { @@ -29,9 +29,9 @@ typedef Vector3 Velocity3; /** * Navigation state: Pose (rotation, translation) + velocity - * Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity + * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold */ -class NavState: public LieGroup { +class NavState { private: // TODO(frank): @@ -42,6 +42,10 @@ private: public: + enum { + dimension = 9 + }; + typedef std::pair PositionAndVelocity; /// @name Constructors @@ -49,7 +53,7 @@ public: /// Default constructor NavState() : - t_(0,0,0), v_(Vector3::Zero()) { + t_(0, 0, 0), v_(Vector3::Zero()) { } /// Construct from attitude, position, velocity NavState(const Rot3& R, const Point3& t, const Velocity3& v) : @@ -59,15 +63,15 @@ public: NavState(const Pose3& pose, const Velocity3& v) : R_(pose.rotation()), t_(pose.translation()), v_(v) { } - /// Construct from Matrix group representation (no checking) - NavState(const Matrix7& T) : - R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) { - } /// Construct from SO(3) and R^6 NavState(const Matrix3& R, const Vector9 tv) : R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { } /// Named constructor with derivatives + static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, + OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 3> H3); + /// Named constructor with derivatives static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); @@ -116,7 +120,8 @@ public: /// @{ /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state); + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const NavState& state); /// print void print(const std::string& s = "") const; @@ -124,29 +129,6 @@ public: /// equals bool equals(const NavState& other, double tol = 1e-8) const; - /// @} - /// @name Group - /// @{ - - /// identity for group operation - static NavState identity() { - return NavState(); - } - - /// inverse transformation with derivatives - NavState inverse() const; - - /// Group compose is the semi-direct product as specified below: - /// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v) - NavState operator*(const NavState& bTc) const; - - /// Native group action is on position/velocity pairs *in the body frame* as follows: - /// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v) - PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const; - - /// Act on position alone, n_t = nRb * b_t + n_t - Point3 operator*(const Point3& b_t) const; - /// @} /// @name Manifold /// @{ @@ -172,44 +154,25 @@ public: return v.segment<3>(6); } - // Chart at origin, constructs components separately (as opposed to Expmap) - struct ChartAtOrigin { - static NavState Retract(const Vector9& xi, // - OptionalJacobian<9, 9> H = boost::none); - static Vector9 Local(const NavState& x, // - OptionalJacobian<9, 9> H = boost::none); - }; + /// retract with optional derivatives + NavState retract(const Vector9& v, // + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = + boost::none) const; - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map at identity - create a NavState from canonical coordinates - static NavState Expmap(const Vector9& xi, // - OptionalJacobian<9, 9> H = boost::none); - - /// Log map at identity - return the canonical coordinates for this NavState - static Vector9 Logmap(const NavState& p, // - OptionalJacobian<9, 9> H = boost::none); - - /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms - /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); - Matrix9 AdjointMap() const; - - /// wedge creates Lie algebra element from tangent vector - static Matrix7 wedge(const Vector9& xi); + /// localCoordinates with optional derivatives + Vector9 localCoordinates(const NavState& g, // + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = + boost::none) const; /// @} /// @name Dynamics /// @{ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const; -#endif /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, @@ -239,14 +202,7 @@ private: // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors template<> -struct traits : Testable, internal::LieGroupTraits { +struct traits : internal::Manifold { }; -// Partial specialization of wedge -// TODO: deprecate, make part of traits -template<> -inline Matrix wedge(const Vector& xi) { - return NavState::wedge(xi); -} - } // namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 88d9c6437..cb40774f4 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -35,6 +35,8 @@ struct PreintegratedRotationParams { PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + virtual ~PreintegratedRotationParams() {} + virtual void print(const std::string& s) const; virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c243ca860..0e8cf67c9 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -31,21 +31,12 @@ namespace gtsam { PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, const Bias& biasHat) : p_(p), biasHat_(biasHat), deltaTij_(0.0) { - resetIntegration(); -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::resetIntegration() { - deltaTij_ = 0.0; - preintegrated_.setZero(); - preintegrated_H_biasAcc_.setZero(); - preintegrated_H_biasOmega_.setZero(); } //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const PreintegrationBase& pim) { os << " deltaTij " << pim.deltaTij_ << endl; - os << " deltaRij " << Point3(pim.theta()) << endl; + os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl; os << " deltaPij " << Point3(pim.deltaPij()) << endl; os << " deltaVij " << Point3(pim.deltaVij()) << endl; os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; @@ -59,14 +50,9 @@ void PreintegrationBase::print(const string& s) const { } //------------------------------------------------------------------------------ -bool PreintegrationBase::equals(const PreintegrationBase& other, - double tol) const { - return p_->equals(*other.p_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol - && biasHat_.equals(other.biasHat_, tol) - && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) - && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) - && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); +void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) { + biasHat_ = biasHat; + resetIntegration(); } //------------------------------------------------------------------------------ @@ -105,7 +91,8 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( // Update derivative: centrifugal causes the correlation between acc and omega!!! if (correctedAcc_H_unbiasedOmega) { double wdp = correctedOmega.dot(b_arm); - *correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal() + const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal(); + *correctedAcc_H_unbiasedOmega = -( diag_wdp + correctedOmega * b_arm.transpose()) * bRs.matrix() + 2 * b_arm * unbiasedOmega.transpose(); } @@ -114,139 +101,38 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( return make_pair(correctedAcc, correctedOmega); } -//------------------------------------------------------------------------------ -// See extensive discussion in ImuFactor.lyx -Vector9 PreintegrationBase::UpdatePreintegrated( - const Vector3& a_body, const Vector3& w_body, double dt, - const Vector9& preintegrated, OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { - const auto theta = preintegrated.segment<3>(0); - const auto position = preintegrated.segment<3>(3); - const auto velocity = preintegrated.segment<3>(6); - - // This functor allows for saving computation when exponential map and its - // derivatives are needed at the same location in so<3> - so3::DexpFunctor local(theta); - - // Calculate exact mean propagation - Matrix3 w_tangent_H_theta, invH; - const Vector3 w_tangent = // angular velocity mapped back to tangent space - local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); - const SO3 R = local.expmap(); - const Vector3 a_nav = R * a_body; - const double dt22 = 0.5 * dt * dt; - - Vector9 preintegratedPlus; - preintegratedPlus << // new preintegrated vector: - theta + w_tangent* dt, // theta - position + velocity* dt + a_nav* dt22, // position - velocity + a_nav* dt; // velocity - - if (A) { - // Exact derivative of R*a with respect to theta: - const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); - - A->setIdentity(); - A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... - A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta - } - if (B) { - B->block<3, 3>(0, 0) = Z_3x3; - B->block<3, 3>(3, 0) = R * dt22; - B->block<3, 3>(6, 0) = R * dt; - } - if (C) { - C->block<3, 3>(0, 0) = invH * dt; - C->block<3, 3>(3, 0) = Z_3x3; - C->block<3, 3>(6, 0) = Z_3x3; - } - - return preintegratedPlus; -} - //------------------------------------------------------------------------------ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt, Matrix9* A, - Matrix93* B, Matrix93* C) { - // Correct for bias in the sensor frame - Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - - // Possibly correct for sensor pose - Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - - // Do update - deltaTij_ += dt; - preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); - - if (p().body_P_sensor) { - // More complicated derivatives in case of non-trivial sensor pose - *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last - } - - // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias - preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); - - // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias - preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); -} - -void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt) { - // NOTE(frank): integrateMeasuremtn always needs to compute the derivatives, + const Vector3& measuredOmega, double dt) { + // NOTE(frank): integrateMeasurement always needs to compute the derivatives, // even when not of interest to the caller. Provide scratch space here. Matrix9 A; Matrix93 B, C; - integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); -} -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::biasCorrectedDelta( - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { - // We correct for a change between bias_i and the biasHat_ used to integrate - // This is a simple linear correction with obvious derivatives - const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - const Vector9 biasCorrected = - preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + - preintegrated_H_biasOmega_ * biasIncr.gyroscope(); - - if (H) { - (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; - } - return biasCorrected; + update(measuredAcc, measuredOmega, dt, &A, &B, &C); } //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, - const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 6> H2) const { + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { // TODO(frank): make sure this stuff is still correct Matrix96 D_biasCorrected_bias; - Vector9 biasCorrected = - biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); + Vector9 biasCorrected = biasCorrectedDelta(bias_i, + H2 ? &D_biasCorrected_bias : 0); // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, - p().omegaCoriolis, p().use2ndOrderCoriolis, - H1 ? &D_delta_state : 0, - H2 ? &D_delta_biasCorrected : 0); + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, + H2 ? &D_delta_biasCorrected : 0); // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); - if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state; - if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias; + if (H1) + *H1 = D_predict_state + D_predict_delta * D_delta_state; + if (H2) + *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; return state_j; } @@ -306,94 +192,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, return error; } -//------------------------------------------------------------------------------ -// sugar for derivative blocks -#define D_R_R(H) (H)->block<3,3>(0,0) -#define D_R_t(H) (H)->block<3,3>(0,3) -#define D_R_v(H) (H)->block<3,3>(0,6) -#define D_t_R(H) (H)->block<3,3>(3,0) -#define D_t_t(H) (H)->block<3,3>(3,3) -#define D_t_v(H) (H)->block<3,3>(3,6) -#define D_v_R(H) (H)->block<3,3>(6,0) -#define D_v_t(H) (H)->block<3,3>(6,3) -#define D_v_v(H) (H)->block<3,3>(6,6) - -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::Compose(const Vector9& zeta01, - const Vector9& zeta12, double deltaT12, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) { - const auto t01 = zeta01.segment<3>(0); - const auto p01 = zeta01.segment<3>(3); - const auto v01 = zeta01.segment<3>(6); - - const auto t12 = zeta12.segment<3>(0); - const auto p12 = zeta12.segment<3>(3); - const auto v12 = zeta12.segment<3>(6); - - Matrix3 R01_H_t01, R12_H_t12; - const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); - const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); - - Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity - const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); - - Matrix3 t02_H_R02; - Vector9 zeta02; - const Matrix3 R = R01.matrix(); - zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta - p01 + v01 * deltaT12 + R * p12, // position - v01 + R * v12; // velocity - - if (H1) { - H1->setIdentity(); - D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; - D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; - D_t_v(H1) = I_3x3 * deltaT12; - D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; - } - - if (H2) { - H2->setZero(); - D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; - D_t_t(H2) = R; - D_v_v(H2) = R; - } - - return zeta02; -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, - Matrix9* H2) { - if (!matchesParamsWith(pim12)) { - throw std::domain_error("Cannot merge pre-integrated measurements with different params"); - } - - if (params()->body_P_sensor) { - throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); - } - - const double t01 = deltaTij(); - const double t12 = pim12.deltaTij(); - deltaTij_ = t01 + t12; - - const Vector9 zeta01 = preintegrated(); - Vector9 zeta12 = pim12.preintegrated(); // will be modified. - - const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); - zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() - + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); - - preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); - - preintegrated_H_biasAcc_ = - (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; - - preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + - (*H2) * pim12.preintegrated_H_biasOmega_; -} - //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, @@ -408,6 +206,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, p_ = q; return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i); } + #endif //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9d751e92d..06be4642d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -75,29 +75,13 @@ class PreintegrationBase { /// Time interval from i to j double deltaTij_; - /** - * Preintegrated navigation state, from frame i to frame j - * Order is: theta, position, velocity - * Note: relative position does not take into account velocity at time i, see deltap+, in [2] - * Note: velocity is now also in frame i, as opposed to deltaVij in [2] - */ - Vector9 preintegrated_; - - Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias - Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias - /// Default constructor for serialization - PreintegrationBase() { - resetIntegration(); - } + PreintegrationBase() {} public: /// @name Constructors /// @{ - /// @name Constructors - /// @{ - /** * Constructor, initializes the variables in the base class * @param p Parameters, typically fixed in a single application @@ -111,7 +95,12 @@ public: /// @name Basic utilities /// @{ /// Re-initialize PreintegratedMeasurements - void resetIntegration(); + virtual void resetIntegration()=0; + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements and set new bias + void resetIntegrationAndSetBias(const Bias& biasHat); /// check parameters equality: checks whether shared pointer points to same Params object. bool matchesParamsWith(const PreintegrationBase& other) const { @@ -140,17 +129,10 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } double deltaTij() const { return deltaTij_; } - const Vector9& preintegrated() const { return preintegrated_; } - - Vector3 theta() const { return preintegrated_.head<3>(); } - Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } - Vector3 deltaVij() const { return preintegrated_.tail<3>(); } - - Rot3 deltaRij() const { return Rot3::Expmap(theta()); } - NavState deltaXij() const { return NavState::Retract(preintegrated_); } - - const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } - const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } + virtual Vector3 deltaPij() const=0; + virtual Vector3 deltaVij() const=0; + virtual Rot3 deltaRij() const=0; + virtual NavState deltaXij() const=0; // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } @@ -159,8 +141,7 @@ public: /// @name Testable /// @{ GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); - void print(const std::string& s) const; - bool equals(const PreintegrationBase& other, double tol) const; + virtual void print(const std::string& s) const; /// @} /// @name Main functionality @@ -175,30 +156,20 @@ public: OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; - // Update integrated vector on tangent manifold preintegrated with acceleration - // Static, functional version. - static Vector9 UpdatePreintegrated(const Vector3& a_body, - const Vector3& w_body, double dt, - const Vector9& preintegrated, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); - /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame - /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT, - Matrix9* A, Matrix93* B, Matrix93* C); + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0; - // Version without derivatives - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT); + /// Version without derivatives + virtual void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far - Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const; + virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const=0; /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, @@ -219,23 +190,6 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; - // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives - static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, - double deltaT12, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none); - - /// Merge in a different set of measurements and update bias derivatives accordingly - /// The derivatives apply to the preintegrated Vector9 - void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2); - /// @} - - /** Dummy clone for MATLAB */ - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(); - } - - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ @@ -249,20 +203,6 @@ public: #endif /// @} - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); - ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); - ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); - } }; } /// namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp new file mode 100644 index 000000000..0ac57d05c --- /dev/null +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -0,0 +1,249 @@ +/* ---------------------------------------------------------------------------- + + * 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 TangentPreintegration.cpp + * @author Frank Dellaert + * @author Adam Bry + **/ + +#include "TangentPreintegration.h" +#include +#include + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +TangentPreintegration::TangentPreintegration(const boost::shared_ptr& p, + const Bias& biasHat) : + PreintegrationBase(p, biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::resetIntegration() { + deltaTij_ = 0.0; + preintegrated_.setZero(); + preintegrated_H_biasAcc_.setZero(); + preintegrated_H_biasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool TangentPreintegration::equals(const TangentPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) + && equal_with_abs_tol(preintegrated_H_biasAcc_, + other.preintegrated_H_biasAcc_, tol) + && equal_with_abs_tol(preintegrated_H_biasOmega_, + other.preintegrated_H_biasOmega_, tol); +} + +//------------------------------------------------------------------------------ +// See extensive discussion in ImuFactor.lyx +Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, double dt, const Vector9& preintegrated, + OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B, + OptionalJacobian<9, 3> C) { + const auto theta = preintegrated.segment<3>(0); + const auto position = preintegrated.segment<3>(3); + const auto velocity = preintegrated.segment<3>(6); + + // This functor allows for saving computation when exponential map and its + // derivatives are needed at the same location in so<3> + so3::DexpFunctor local(theta); + + // Calculate exact mean propagation + Matrix3 w_tangent_H_theta, invH; + const Vector3 w_tangent = // angular velocity mapped back to tangent space + local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); + const SO3 R = local.expmap(); + const Vector3 a_nav = R * a_body; + const double dt22 = 0.5 * dt * dt; + + Vector9 preintegratedPlus; + preintegratedPlus << // new preintegrated vector: + theta + w_tangent * dt, // theta + position + velocity * dt + a_nav * dt22, // position + velocity + a_nav * dt; // velocity + + if (A) { + // Exact derivative of R*a with respect to theta: + const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); + + A->setIdentity(); + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta + } + if (B) { + B->block<3, 3>(0, 0) = Z_3x3; + B->block<3, 3>(3, 0) = R * dt22; + B->block<3, 3>(6, 0) = R * dt; + } + if (C) { + C->block<3, 3>(0, 0) = invH * dt; + C->block<3, 3>(3, 0) = Z_3x3; + C->block<3, 3>(6, 0) = Z_3x3; + } + + return preintegratedPlus; +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Do update + deltaTij_ += dt; + preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc + // where acc_H_biasAcc = -I_3x3, hence + // new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc + preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); + + // new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega + // where omega_H_biasOmega = -I_3x3, hence + // new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega + preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); +} + +//------------------------------------------------------------------------------ +Vector9 TangentPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // We correct for a change between bias_i and the biasHat_ used to integrate + // This is a simple linear correction with obvious derivatives + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + const Vector9 biasCorrected = preintegrated() + + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + + preintegrated_H_biasOmega_ * biasIncr.gyroscope(); + + if (H) { + (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; + } + return biasCorrected; +} + +//------------------------------------------------------------------------------ +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) + +//------------------------------------------------------------------------------ +Vector9 TangentPreintegration::Compose(const Vector9& zeta01, + const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) { + const auto t01 = zeta01.segment<3>(0); + const auto p01 = zeta01.segment<3>(3); + const auto v01 = zeta01.segment<3>(6); + + const auto t12 = zeta12.segment<3>(0); + const auto p12 = zeta12.segment<3>(3); + const auto v12 = zeta12.segment<3>(6); + + Matrix3 R01_H_t01, R12_H_t12; + const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); + const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); + + Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity + const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); + + Matrix3 t02_H_R02; + Vector9 zeta02; + const Matrix3 R = R01.matrix(); + zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta + p01 + v01 * deltaT12 + R * p12, // position + v01 + R * v12; // velocity + + if (H1) { + H1->setIdentity(); + D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; + D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; + D_t_v(H1) = I_3x3 * deltaT12; + D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; + } + + if (H2) { + H2->setZero(); + D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; + D_t_t(H2) = R; + D_v_v(H2) = R; + } + + return zeta02; +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, + Matrix9* H1, Matrix9* H2) { + if (!matchesParamsWith(pim12)) { + throw std::domain_error( + "Cannot merge pre-integrated measurements with different params"); + } + + if (params()->body_P_sensor) { + throw std::domain_error( + "Cannot merge pre-integrated measurements with sensor pose yet"); + } + + const double t01 = deltaTij(); + const double t12 = pim12.deltaTij(); + deltaTij_ = t01 + t12; + + const Vector9 zeta01 = preintegrated(); + Vector9 zeta12 = pim12.preintegrated(); // will be modified. + + const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); + zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() + + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); + + preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2); + + preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_ + + (*H2) * pim12.preintegrated_H_biasAcc_; + + preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + + (*H2) * pim12.preintegrated_H_biasOmega_; +} + +//------------------------------------------------------------------------------ + +}// namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h new file mode 100644 index 000000000..dddafad7a --- /dev/null +++ b/gtsam/navigation/TangentPreintegration.h @@ -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 TangentPreintegration.h + * @author Frank Dellaert + * @author Adam Bry + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * Integrate on the 9D tangent space of the NavState manifold. + * See extensive discussion in ImuFactor.lyx + */ +class TangentPreintegration : public PreintegrationBase { + protected: + + /** + * Preintegrated navigation state, as a 9D vector on tangent space at frame i + * Order is: theta, position, velocity + */ + Vector9 preintegrated_; + Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated_ w.r.t. acceleration bias + Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated_ w.r.t. angular rate bias + + /// Default constructor for serialization + TangentPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + TangentPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } + Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } + Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } + NavState deltaXij() const override { return NavState().retract(preintegrated_); } + + const Vector9& preintegrated() const { return preintegrated_; } + Vector3 theta() const { return preintegrated_.head<3>(); } + const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } + const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } + + /// @name Testable + /// @{ + bool equals(const TangentPreintegration& other, double tol) const; + /// @} + + /// @name Main functionality + /// @{ + + // Update integrated vector on tangent manifold preintegrated with acceleration + // Static, functional version. + static Vector9 UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, const double dt, + const Vector9& preintegrated, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override; + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives + static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, + double deltaT12, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none); + + /// Merge in a different set of measurements and update bias derivatives accordingly + /// The derivatives apply to the preintegrated Vector9 + void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2); + /// @} + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); + ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); + ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fa2e9d832..c1c17a6d4 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -132,6 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { } /* ************************************************************************* */ +#ifdef GTSAM_TANGENT_PREINTEGRATION TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { auto p = testing::Params(); testing::SomeMeasurements measurements; @@ -151,6 +152,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasOmega(), 1e-3)); } +#endif /* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity) { diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index d9ba38e02..293bffa00 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -58,7 +58,7 @@ TEST( GPSFactor, Constructor ) { // Create a linearization point at zero error Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); - EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -87,7 +87,7 @@ TEST( GPSFactor2, Constructor ) { // Create a linearization point at zero error NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); - EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 54ca50797..6f2d60f2f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -57,6 +57,41 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, } // namespace +/* ************************************************************************* */ +TEST(ImuFactor, PreintegratedMeasurementsConstruction) { + // Actual pre-integrated values + PreintegratedImuMeasurements actual(testing::Params()); + EXPECT(assert_equal(Rot3(), actual.deltaRij())); + EXPECT(assert_equal(kZero, actual.deltaPij())); + EXPECT(assert_equal(kZero, actual.deltaVij())); + DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PreintegratedMeasurementsReset) { + + auto p = testing::Params(); + // Create a preintegrated measurement struct and integrate + PreintegratedImuMeasurements pimActual(p); + Vector3 measuredAcc(0.5, 1.0, 0.5); + Vector3 measuredOmega(0.1, 0.3, 0.1); + double deltaT = 1.0; + pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // reset and make sure that it is the same as a fresh one + pimActual.resetIntegration(); + CHECK(assert_equal(pimActual, PreintegratedImuMeasurements(p))); + + // Now create one with a different bias .. + Bias nonZeroBias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); + PreintegratedImuMeasurements pimExpected(p, nonZeroBias); + + // integrate again, then reset to a new bias + pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pimActual.resetIntegrationAndSetBias(nonZeroBias); + CHECK(assert_equal(pimActual, pimExpected)); +} + /* ************************************************************************* */ TEST(ImuFactor, Accelerating) { const double a = 0.2, v = 50; @@ -83,24 +118,20 @@ TEST(ImuFactor, Accelerating) { /* ************************************************************************* */ TEST(ImuFactor, PreintegratedMeasurements) { // Measurements - Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); + const double a = 0.1, w = M_PI / 100.0; + Vector3 measuredAcc(a, 0.0, 0.0); + Vector3 measuredOmega(w, 0.0, 0.0); double deltaT = 0.5; // Expected pre-integrated values - Vector3 expectedDeltaR1(0.5 * M_PI / 100.0, 0.0, 0.0); - Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 0, 0); + Vector3 expectedDeltaR1(w * deltaT, 0.0, 0.0); + Vector3 expectedDeltaP1(0.5 * a * deltaT*deltaT, 0, 0); Vector3 expectedDeltaV1(0.05, 0.0, 0.0); // Actual pre-integrated values PreintegratedImuMeasurements actual(testing::Params()); - EXPECT(assert_equal(kZero, actual.theta())); - EXPECT(assert_equal(kZero, actual.deltaPij())); - EXPECT(assert_equal(kZero, actual.deltaVij())); - DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); - actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(expectedDeltaR1, actual.theta())); + EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij())); EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij())); EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij())); DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9); @@ -129,7 +160,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { // Actual pre-integrated values actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(expectedDeltaR2, actual.theta())); + EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij())); EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij())); EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij())); DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9); @@ -438,27 +469,6 @@ TEST(ImuFactor, fistOrderExponential) { EXPECT(assert_equal(expectedRot, actualRot)); } -/* ************************************************************************* */ -TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - testing::SomeMeasurements measurements; - - boost::function preintegrated = - [=](const Vector3& a, const Vector3& w) { - PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w)); - testing::integrateMeasurements(measurements, &pim); - return pim.preintegrated(); - }; - - // Actual pre-integrated values - PreintegratedImuMeasurements pim(testing::Params()); - testing::integrateMeasurements(measurements, &pim); - - EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero), - pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero), - pim.preintegrated_H_biasOmega(), 1e-3)); -} - /* ************************************************************************* */ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { @@ -771,8 +781,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factors - graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim)); - graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); + graph.emplace_shared(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim); + graph.emplace_shared >(B(i - 1), B(i), zeroBias, biasNoiseModel); values.insert(X(i), Pose3()); values.insert(V(i), zeroVel); @@ -789,6 +799,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { } /* ************************************************************************* */ +#ifdef GTSAM_TANGENT_PREINTEGRATION static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6; struct ImuFactorMergeTest { @@ -883,6 +894,7 @@ TEST(ImuFactor, MergeWithCoriolis) { mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1); mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); } +#endif /* ************************************************************************* */ // Same values as pre-integration test but now testing covariance diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 1cc84751c..45db58e33 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -71,19 +71,19 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); - EXPECT( assert_equal(Z_3x3,f.evaluateError(theta,H1),1e-5)); + EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); - EXPECT( assert_equal(Z_3x3,f1.evaluateError(nRb,H1),1e-5)); + EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); - EXPECT( assert_equal(Z_3x3,f2.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); @@ -93,7 +93,7 @@ TEST( MagFactor, Factors ) { // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); - EXPECT(assert_equal(Z_3x3,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); + EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp new file mode 100644 index 000000000..a3f688dda --- /dev/null +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testManifoldPreintegration.cpp + * @brief Unit test for the ManifoldPreintegration + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include "imuFactorTesting.h" + +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} + +/* ************************************************************************* */ +TEST(ManifoldPreintegration, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; + + boost::function deltaRij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaRij(); + }; + + boost::function deltaPij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaPij(); + }; + + boost::function deltaVij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaVij(); + }; + + // Actual pre-integrated values + ManifoldPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(deltaRij, kZero, kZero), + Matrix3(Z_3x3))); + EXPECT( + assert_equal(numericalDerivative22(deltaRij, kZero, kZero), + pim.delRdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaPij, kZero, kZero), + pim.delPdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaPij, kZero, kZero), + pim.delPdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaVij, kZero, kZero), + pim.delVdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaVij, kZero, kZero), + pim.delVdelBiasOmega(), 1e-3)); +} + +/* ************************************************************************* */ +TEST(ManifoldPreintegration, computeError) { + ManifoldPreintegration pim(testing::Params()); + NavState x1, x2; + imuBias::ConstantBias bias; + Matrix9 aH1, aH2; + Matrix96 aH3; + pim.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index fb6045d33..8ea8ce9b5 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero(); /* ************************************************************************* */ TEST(NavState, Constructor) { + boost::function create = + boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none, + boost::none); + Matrix aH1, aH2, aH3; + EXPECT( + assert_equal(kState1, + NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); + EXPECT( + assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); + EXPECT( + assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + EXPECT( + assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +} + +/* ************************************************************************* */ +TEST(NavState, Constructor2) { boost::function construct = boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::none); @@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) { EXPECT(assert_equal((Matrix )eH, aH)); } -/* ************************************************************************* */ -TEST( NavState, MatrixGroup ) { - // check roundtrip conversion to 7*7 matrix representation - Matrix7 T = kState1.matrix(); - EXPECT(assert_equal(kState1, NavState(T))); - - // check group product agrees with matrix product - NavState state2 = kState1 * kState1; - Matrix T2 = T * T; - EXPECT(assert_equal(state2, NavState(T2))); - EXPECT(assert_equal(T2, state2.matrix())); -} - /* ************************************************************************* */ TEST( NavState, Manifold ) { // Check zero xi @@ -114,7 +121,9 @@ TEST( NavState, Manifold ) { Rot3 drot = Rot3::Expmap(xi.head<3>()); Point3 dt = Point3(xi.segment<3>(3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); - NavState state2 = kState1 * NavState(drot, dt, dvel); + NavState state2 = NavState(kState1.attitude() * drot, + kState1.position() + kState1.attitude() * dt, + kState1.velocity() + kState1.attitude() * dvel); EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -122,27 +131,6 @@ TEST( NavState, Manifold ) { NavState state3 = state2.retract(xi); EXPECT(assert_equal(xi, state2.localCoordinates(state3))); - // Check derivatives for ChartAtOrigin::Retract - Matrix9 aH; - // For zero xi - boost::function Retract = boost::bind( - NavState::ChartAtOrigin::Retract, _1, boost::none); - NavState::ChartAtOrigin::Retract(kZeroXi, aH); - EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH)); - // For non-zero xi - NavState::ChartAtOrigin::Retract(xi, aH); - EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH)); - - // Check derivatives for ChartAtOrigin::Local - // For zero xi - boost::function Local = boost::bind( - NavState::ChartAtOrigin::Local, _1, boost::none); - NavState::ChartAtOrigin::Local(kIdentity, aH); - EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH)); - // For non-zero xi - NavState::ChartAtOrigin::Local(kState1, aH); - EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH)); - // Check retract derivatives Matrix9 aH1, aH2; kState1.retract(xi, aH1, aH2); @@ -151,6 +139,12 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); + // Check retract derivatives on state 2 + const Vector9 xi2 = -3.0*xi; + state2.retract(xi2, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1)); + EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2)); + // Check localCoordinates derivatives boost::function local = boost::bind(&NavState::localCoordinates, _1, _2, boost::none, @@ -169,29 +163,6 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } -/* ************************************************************************* */ -TEST( NavState, Lie ) { - // Check zero xi - EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi))); - EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity))); - EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi))); - EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1))); - - // Expmap/Logmap roundtrip - Vector xi(9); - xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; - NavState state2 = NavState::Expmap(xi); - EXPECT(assert_equal(xi, NavState::Logmap(state2))); - - // roundtrip from state2 to state3 and back - NavState state3 = state2.expmap(xi); - EXPECT(assert_equal(xi, state2.logmap(state3))); - - // For the expmap/logmap (not necessarily expmap/local) -xi goes other way - EXPECT(assert_equal(state2, state3.expmap(-xi))); - EXPECT(assert_equal(xi, -state3.logmap(state2))); -} - /* ************************************************************************* */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 TEST(NavState, Update) { @@ -201,8 +172,8 @@ TEST(NavState, Update) { Matrix9 aF; Matrix93 aG1, aG2; boost::function update = - boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, - boost::none, boost::none); + boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, + boost::none, boost::none); Vector3 b_acc = kAttitude * acc; NavState expected(kAttitude.expmap(dt * omega), kPosition + Point3((kVelocity + b_acc * dt / 2) * dt), diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp similarity index 72% rename from gtsam/navigation/tests/testPreintegrationBase.cpp rename to gtsam/navigation/tests/testTangentPreintegration.cpp index 527a6da7b..65fd55fa3 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -10,12 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file testPreintegrationBase.cpp + * @file testTangentPreintegration.cpp * @brief Unit test for the InertialNavFactor * @author Frank Dellaert */ -#include +#include #include #include #include @@ -29,7 +29,7 @@ static const double kDt = 0.1; Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { - return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); + return TangentPreintegration::UpdatePreintegrated(a, w, kDt, zeta); } namespace testing { @@ -44,8 +44,8 @@ static boost::shared_ptr Params() { } /* ************************************************************************* */ -TEST(PreintegrationBase, UpdateEstimate1) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, UpdateEstimate1) { + TangentPreintegration pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta.setZero(); @@ -58,8 +58,8 @@ TEST(PreintegrationBase, UpdateEstimate1) { } /* ************************************************************************* */ -TEST(PreintegrationBase, UpdateEstimate2) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, UpdateEstimate2) { + TangentPreintegration pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; @@ -73,8 +73,31 @@ TEST(PreintegrationBase, UpdateEstimate2) { } /* ************************************************************************* */ -TEST(PreintegrationBase, computeError) { - PreintegrationBase pim(testing::Params()); +TEST(ImuFactor, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; + + boost::function preintegrated = + [=](const Vector3& a, const Vector3& w) { + TangentPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.preintegrated(); + }; + + // Actual pre-integrated values + TangentPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(preintegrated, kZero, kZero), + pim.preintegrated_H_biasAcc())); + EXPECT( + assert_equal(numericalDerivative22(preintegrated, kZero, kZero), + pim.preintegrated_H_biasOmega(), 1e-3)); +} + +/* ************************************************************************* */ +TEST(TangentPreintegration, computeError) { + TangentPreintegration pim(testing::Params()); NavState x1, x2; imuBias::ConstantBias bias; Matrix9 aH1, aH2; @@ -82,7 +105,7 @@ TEST(PreintegrationBase, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3, + boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3, boost::none, boost::none, boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); @@ -91,47 +114,47 @@ TEST(PreintegrationBase, computeError) { } /* ************************************************************************* */ -TEST(PreintegrationBase, Compose) { +TEST(TangentPreintegration, Compose) { testing::SomeMeasurements measurements; - PreintegrationBase pim(testing::Params()); + TangentPreintegration pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); boost::function f = [pim](const Vector9& zeta01, const Vector9& zeta12) { - return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij()); + return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij()); }; // Expected merge result - PreintegrationBase expected_pim02(testing::Params()); + TangentPreintegration expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); // Actual result Matrix9 H1, H2; - PreintegrationBase actual_pim02 = pim; + TangentPreintegration actual_pim02 = pim; actual_pim02.mergeWith(pim, &H1, &H2); const Vector9 zeta = pim.preintegrated(); const Vector9 actual_zeta = - PreintegrationBase::Compose(zeta, zeta, pim.deltaTij()); + TangentPreintegration::Compose(zeta, zeta, pim.deltaTij()); EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7)); EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7)); EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7)); } /* ************************************************************************* */ -TEST(PreintegrationBase, MergedBiasDerivatives) { +TEST(TangentPreintegration, MergedBiasDerivatives) { testing::SomeMeasurements measurements; auto f = [=](const Vector3& a, const Vector3& w) { - PreintegrationBase pim02(testing::Params(), Bias(a, w)); + TangentPreintegration pim02(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim02); testing::integrateMeasurements(measurements, &pim02); return pim02.preintegrated(); }; // Expected merge result - PreintegrationBase expected_pim02(testing::Params()); + TangentPreintegration expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index a91515e9c..97326f06c 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -25,8 +26,6 @@ #include -using namespace std; - namespace gtsam { /* ************************************************************************* */ @@ -51,10 +50,41 @@ std::string DoglegParams::verbosityDLTranslator(VerbosityDL verbosityDL) const { } /* ************************************************************************* */ -void DoglegOptimizer::iterate(void) { +namespace internal { +struct DoglegState : public NonlinearOptimizerState { + const double delta; + + DoglegState(const Values& values, double error, double delta, unsigned int iterations = 0) + : NonlinearOptimizerState(values, error, iterations), delta(delta) {} +}; +} + +typedef internal::DoglegState State; + +/* ************************************************************************* */ +DoglegOptimizer::DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const DoglegParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr( + new State(initialValues, graph.error(initialValues), params.deltaInitial))), + params_(ensureHasOrdering(params, graph)) {} + +DoglegOptimizer::DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const Ordering& ordering) + : NonlinearOptimizer(graph, std::unique_ptr( + new State(initialValues, graph.error(initialValues), 1.0))) { + params_.ordering = ordering; +} + +double DoglegOptimizer::getDelta() const { + return static_cast(state_.get())->delta; +} + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr DoglegOptimizer::iterate(void) { // Linearize graph - GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values); + GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_->values); // Pull out parameters we'll use const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT); @@ -66,31 +96,30 @@ void DoglegOptimizer::iterate(void) { GaussianBayesTree bt = *linear->eliminateMultifrontal(*params_.ordering, params_.getEliminationFunction()); VectorValues dx_u = bt.optimizeGradientSearch(); VectorValues dx_n = bt.optimize(); - result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, - dx_u, dx_n, bt, graph_, state_.values, state_.error, dlVerbose); + result = DoglegOptimizerImpl::Iterate(getDelta(), DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, + dx_u, dx_n, bt, graph_, state_->values, state_->error, dlVerbose); } else if ( params_.isSequential() ) { GaussianBayesNet bn = *linear->eliminateSequential(*params_.ordering, params_.getEliminationFunction()); VectorValues dx_u = bn.optimizeGradientSearch(); VectorValues dx_n = bn.optimize(); - result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, - dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose); + result = DoglegOptimizerImpl::Iterate(getDelta(), DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, + dx_u, dx_n, bn, graph_, state_->values, state_->error, dlVerbose); } else if ( params_.isIterative() ) { - throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver"); + throw std::runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver"); } else { - throw runtime_error("Optimization parameter is invalid: DoglegParams::elimination"); + throw std::runtime_error("Optimization parameter is invalid: DoglegParams::elimination"); } // Maybe show output if(params_.verbosity >= NonlinearOptimizerParams::DELTA) result.dx_d.print("delta"); // Create new state with new values and new error - state_.values = state_.values.retract(result.dx_d); - state_.error = result.f_error; - state_.Delta = result.Delta; - ++state_.iterations; + state_.reset(new State(state_->values.retract(result.dx_d), result.f_error, result.delta, + state_->iterations + 1)); + return linear; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 8a43b4d96..7013908e5 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -11,7 +11,7 @@ /** * @file DoglegOptimizer.h - * @brief + * @brief * @author Richard Roberts * @date Feb 26, 2012 */ @@ -45,7 +45,7 @@ public: virtual ~DoglegParams() {} - virtual void print(const std::string& str = "") const { + void print(const std::string& str = "") const override { NonlinearOptimizerParams::print(str); std::cout << " deltaInitial: " << deltaInitial << "\n"; std::cout.flush(); @@ -62,24 +62,6 @@ private: std::string verbosityDLTranslator(VerbosityDL verbosityDL) const; }; -/** - * State for DoglegOptimizer - */ -class GTSAM_EXPORT DoglegState : public NonlinearOptimizerState { -public: - double Delta; - - DoglegState() {} - - virtual ~DoglegState() {} - -protected: - DoglegState(const NonlinearFactorGraph& graph, const Values& values, const DoglegParams& params, unsigned int iterations = 0) : - NonlinearOptimizerState(graph, values, iterations), Delta(params.deltaInitial) {} - - friend class DoglegOptimizer; -}; - /** * This class performs Dogleg nonlinear optimization */ @@ -87,7 +69,6 @@ class GTSAM_EXPORT DoglegOptimizer : public NonlinearOptimizer { protected: DoglegParams params_; - DoglegState state_; public: typedef boost::shared_ptr shared_ptr; @@ -104,8 +85,7 @@ public: * @param params The optimization parameters */ DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const DoglegParams& params = DoglegParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_(graph, initialValues, params_) {} + const DoglegParams& params = DoglegParams()); /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -114,10 +94,8 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) : - NonlinearOptimizer(graph) { - params_.ordering = ordering; - state_ = DoglegState(graph, initialValues, params_); } + DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const Ordering& ordering); /// @} @@ -131,31 +109,19 @@ public: * containing the updated variable assignments, which may be retrieved with * values(). */ - virtual void iterate(); + GaussianFactorGraph::shared_ptr iterate() override; /** Read-only access the parameters */ const DoglegParams& params() const { return params_; } - /** Read/write access the parameters. */ - DoglegParams& params() { return params_; } - - /** Read-only access the last state */ - const DoglegState& state() const { return state_; } - - /** Read/write access the last state. When modifying the state, the error, etc. must be consistent before calling iterate() */ - DoglegState& state() { return state_; } - - /** Access the current trust region radius Delta */ - double getDelta() const { return state_.Delta; } + /** Access the current trust region radius delta */ + double getDelta() const; /// @} protected: /** Access the parameters (base class version) */ - virtual const NonlinearOptimizerParams& _params() const { return params_; } - - /** Access the state (base class version) */ - virtual const NonlinearOptimizerState& _state() const { return state_; } + virtual const NonlinearOptimizerParams& _params() const override { return params_; } /** Internal function for computing a COLAMD ordering if no ordering is specified */ DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const; diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 671dfe8f8..c319f26e6 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -23,24 +23,24 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint( - double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose) { + double delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose) { - // Get magnitude of each update and find out which segment Delta falls in - assert(Delta >= 0.0); - double DeltaSq = Delta*Delta; + // Get magnitude of each update and find out which segment delta falls in + assert(delta >= 0.0); + double deltaSq = delta*delta; double x_u_norm_sq = dx_u.squaredNorm(); double x_n_norm_sq = dx_n.squaredNorm(); if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl; - if(DeltaSq < x_u_norm_sq) { + if(deltaSq < x_u_norm_sq) { // Trust region is smaller than steepest descent update - VectorValues x_d = std::sqrt(DeltaSq / x_u_norm_sq) * dx_u; - if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl; + VectorValues x_d = std::sqrt(deltaSq / x_u_norm_sq) * dx_u; + if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(deltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl; return x_d; - } else if(DeltaSq < x_n_norm_sq) { + } else if(deltaSq < x_n_norm_sq) { // Trust region boundary is between steepest descent point and Newton's method point - return ComputeBlend(Delta, dx_u, dx_n, verbose); + return ComputeBlend(delta, dx_u, dx_n, verbose); } else { - assert(DeltaSq >= x_n_norm_sq); + assert(deltaSq >= x_n_norm_sq); if(verbose) cout << "In pure Newton's method region" << endl; // Trust region is larger than Newton's method point return dx_n; @@ -48,7 +48,7 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint( } /* ************************************************************************* */ -VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose) { +VectorValues DoglegOptimizerImpl::ComputeBlend(double delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose) { // See doc/trustregion.lyx or doc/trustregion.pdf @@ -60,7 +60,7 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues& // Compute quadratic formula terms const double a = uu - 2.*un + nn; const double b = 2. * (un - uu); - const double c = uu - Delta*Delta; + const double c = uu - delta*delta; double sqrt_b_m4ac = std::sqrt(b*b - 4*a*c); // Compute blending parameter diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index dea4113f7..9a7067878 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -32,7 +32,7 @@ namespace gtsam { struct GTSAM_EXPORT DoglegOptimizerImpl { struct GTSAM_EXPORT IterationResult { - double Delta; + double delta; VectorValues dx_d; double f_error; }; @@ -58,7 +58,7 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { /** * Compute the update point for one iteration of the Dogleg algorithm, given - * an initial trust region radius \f$ \Delta \f$. The trust region radius is + * an initial trust region radius \f$ \delta \f$. The trust region radius is * adapted based on the error of a NonlinearFactorGraph \f$ f(x) \f$, and * depending on the update mode \c mode. * @@ -80,24 +80,24 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { * @tparam F For normal usage this will be NonlinearFactorGraph. * @tparam VALUES The Values or TupleValues to pass to F::error() to evaluate * the error function. - * @param Delta The initial trust region radius. + * @param delta The initial trust region radius. * @param mode See DoglegOptimizerImpl::TrustRegionAdaptationMode * @param Rd The Bayes' net or tree as described above. * @param f The original nonlinear factor graph with which to evaluate the - * accuracy of \f$ M(\delta x) \f$ to adjust \f$ \Delta \f$. + * accuracy of \f$ M(\delta x) \f$ to adjust \f$ \delta \f$. * @param x0 The linearization point about which \f$ \bayesNet \f$ was created * @param ordering The variable ordering used to create\f$ \bayesNet \f$ * @param f_error The result of f.error(x0). - * @return A DoglegIterationResult containing the new \c Delta, the linear + * @return A DoglegIterationResult containing the new \c delta, the linear * update \c dx_d, and the resulting nonlinear error \c f_error. */ template static IterationResult Iterate( - double Delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n, + double delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n, const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose=false); /** - * Compute the dogleg point given a trust region radius \f$ \Delta \f$. The + * Compute the dogleg point given a trust region radius \f$ \delta \f$. The * dogleg point is the intersection between the dogleg path and the trust * region boundary, see doc/trustregion.pdf for more details. * @@ -113,30 +113,30 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { * upper-triangular and \f$ d \f$ is a vector, in GTSAM represented as a * GaussianBayesNet, containing GaussianConditional s. * - * @param Delta The trust region radius + * @param delta The trust region radius * @param dx_u The steepest descent point, i.e. the Cauchy point * @param dx_n The Gauss-Newton point * @return The dogleg point \f$ \delta x_d \f$ */ - static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false); + static VectorValues ComputeDoglegPoint(double delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false); /** Compute the point on the line between the steepest descent point and the * Newton's method point intersecting the trust region boundary. * Mathematically, computes \f$ \tau \f$ such that \f$ 0<\tau<1 \f$ and - * \f$ \| (1-\tau)\delta x_u + \tau\delta x_n \| = \Delta \f$, where \f$ \Delta \f$ + * \f$ \| (1-\tau)\delta x_u + \tau\delta x_n \| = \delta \f$, where \f$ \delta \f$ * is the trust region radius. - * @param Delta Trust region radius + * @param delta Trust region radius * @param x_u Steepest descent minimizer * @param x_n Newton's method minimizer */ - static VectorValues ComputeBlend(double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false); + static VectorValues ComputeBlend(double delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false); }; /* ************************************************************************* */ template typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( - double Delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n, + double delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n, const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose) { gttic(M_error); @@ -151,10 +151,10 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( while(stay) { gttic(Dog_leg_point); // Compute dog leg point - result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n, verbose); + result.dx_d = ComputeDoglegPoint(delta, dx_u, dx_n, verbose); gttoc(Dog_leg_point); - if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl; + if(verbose) std::cout << "delta = " << delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl; gttic(retract); // Compute expmapped solution @@ -174,7 +174,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << " -> " << result.f_error << std::endl; if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << " -> " << new_M_error << std::endl; - gttic(adjust_Delta); + gttic(adjust_delta); // Compute gain ratio. Here we take advantage of the invariant that the // Bayes' net error at zero is equal to the nonlinear error const double rho = fabs(f_error - result.f_error) < 1e-15 || fabs(M_error - new_M_error) < 1e-15 ? @@ -186,35 +186,35 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(rho >= 0.75) { // M agrees very well with f, so try to increase lambda const double dx_d_norm = result.dx_d.norm(); - const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta + const double newDelta = std::max(delta, 3.0 * dx_d_norm); // Compute new delta if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) - stay = false; // If not searching, just return with the new Delta + stay = false; // If not searching, just return with the new delta else if(mode == SEARCH_EACH_ITERATION) { - if(fabs(newDelta - Delta) < 1e-15 || lastAction == DECREASED_DELTA) + if(fabs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA) stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region else { - stay = true; // Searching and increased Delta, so try again to increase Delta + stay = true; // Searching and increased delta, so try again to increase delta lastAction = INCREASED_DELTA; } } else { assert(false); } - Delta = newDelta; // Update Delta from new Delta + delta = newDelta; // Update delta from new delta } else if(0.75 > rho && rho >= 0.25) { - // M agrees so-so with f, keep the same Delta + // M agrees so-so with f, keep the same delta stay = false; } else if(0.25 > rho && rho >= 0.0) { - // M does not agree well with f, decrease Delta until it does + // M does not agree well with f, decrease delta until it does double newDelta; bool hitMinimumDelta; - if(Delta > 1e-5) { - newDelta = 0.5 * Delta; + if(delta > 1e-5) { + newDelta = 0.5 * delta; hitMinimumDelta = false; } else { - newDelta = Delta; + newDelta = delta; hitMinimumDelta = true; } if(mode == ONE_STEP_PER_ITERATION || /* mode == SEARCH_EACH_ITERATION && */ lastAction == INCREASED_DELTA || hitMinimumDelta) @@ -225,29 +225,29 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( } else { assert(false); } - Delta = newDelta; // Update Delta from new Delta + delta = newDelta; // Update delta from new delta } else { - // f actually increased, so keep decreasing Delta until f does not decrease. + // f actually increased, so keep decreasing delta until f does not decrease. // NOTE: NaN and Inf solutions also will fall into this case, so that we - // decrease Delta if the solution becomes undetermined. + // decrease delta if the solution becomes undetermined. assert(0.0 > rho); - if(Delta > 1e-5) { - Delta *= 0.5; + if(delta > 1e-5) { + delta *= 0.5; stay = true; lastAction = DECREASED_DELTA; } else { - if(verbose) std::cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << std::endl; + if(verbose) std::cout << "Warning: Dog leg stopping because cannot decrease error with minimum delta" << std::endl; result.dx_d.setZero(); // Set delta to zero - don't allow error to increase result.f_error = f_error; stay = false; } } - gttoc(adjust_Delta); + gttoc(adjust_delta); } // dx_d and f_error have already been filled in during the loop - result.Delta = Delta; + result.delta = delta; return result; } diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index e59201ca7..259bb1efe 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -263,26 +263,15 @@ template ScalarMultiplyExpression::ScalarMultiplyExpression(double s, const Expression& e) : Expression(boost::make_shared>(s, e)) {} -template -SumExpression::SumExpression(const std::vector>& expressions) - : Expression(boost::make_shared>(expressions)) {} template -SumExpression SumExpression::operator+(const Expression& e) const { - SumExpression copy = *this; - boost::static_pointer_cast>(copy.root_)->add(e); - return copy; -} +BinarySumExpression::BinarySumExpression(const Expression& e1, const Expression& e2) + : Expression(boost::make_shared>(e1, e2)) {} template -SumExpression& SumExpression::operator+=(const Expression& e) { - boost::static_pointer_cast>(this->root_)->add(e); +Expression& Expression::operator+=(const Expression& e) { + root_ = boost::make_shared>(*this, e); return *this; } -template -size_t SumExpression::nrTerms() const { - return boost::static_pointer_cast>(this->root_)->nrTerms(); -} - } // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 6ed6bb4a5..7d02d479c 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -174,6 +174,9 @@ public: /// Return size needed for memory buffer in traceExecution size_t traceSize() const; + /// Add another expression to this expression + Expression& operator+=(const Expression& e); + protected: /// Default constructor, for serialization @@ -217,24 +220,19 @@ class ScalarMultiplyExpression : public Expression { }; /** - * A SumExpression is a specialization of Expression that just sums the arguments + * A BinarySumExpression is a specialization of Expression that adds two expressions together * It optimizes the Jacobian calculation for this specific case */ template -class SumExpression : public Expression { +class BinarySumExpression : public Expression { // Check that T is a vector space BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); public: - explicit SumExpression(const std::vector>& expressions); - - // Syntactic sugar to allow e1 + e2 + e3... - SumExpression operator+(const Expression& e) const; - SumExpression& operator+=(const Expression& e); - - size_t nrTerms() const; + explicit BinarySumExpression(const Expression& e1, const Expression& e2); }; + /** * Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA * TODO(frank): create a more efficient version like ScalarMultiplyExpression. This version still @@ -272,13 +270,14 @@ ScalarMultiplyExpression operator*(double s, const Expression& e) { * Expression a(0), b(1), c = a + b; */ template -SumExpression operator+(const Expression& e1, const Expression& e2) { - return SumExpression({e1, e2}); +BinarySumExpression operator+(const Expression& e1, const Expression& e2) { + return BinarySumExpression(e1, e2); } /// Construct an expression that subtracts one expression from another template -SumExpression operator-(const Expression& e1, const Expression& e2) { +BinarySumExpression operator-(const Expression& e1, const Expression& e2) { + // TODO(frank, abe): Implement an actual negate operator instead of multiplying by -1 return e1 + (-1.0) * e2; } diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index e0c4da94e..e85aceb15 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -61,16 +61,14 @@ namespace gtsam { } /* ************************************************************************* */ - template - ExtendedKalmanFilter::ExtendedKalmanFilter(Key key_initial, T x_initial, - noiseModel::Gaussian::shared_ptr P_initial) { - - // Set the initial linearization point to the provided mean - x_ = x_initial; - + template + ExtendedKalmanFilter::ExtendedKalmanFilter( + Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial) + : x_(x_initial) // Set the initial linearization point + { // Create a Jacobian Prior Factor directly P_initial. // Since x0 is set to the provided mean, the b vector in the prior will be zero - // TODO Frank asks: is there a reason why noiseModel is not simply P_initial ? + // TODO(Frank): is there a reason why noiseModel is not simply P_initial? int n = traits::GetDimension(x_initial); priorFactor_ = JacobianFactor::shared_ptr( new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n), diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index d12c56b6f..c31451e56 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -17,36 +17,52 @@ */ #include +#include #include #include -using namespace std; - namespace gtsam { -/* ************************************************************************* */ -void GaussNewtonOptimizer::iterate() { - gttic(GaussNewtonOptimizer_Iterate); +typedef internal::NonlinearOptimizerState State; - const NonlinearOptimizerState& current = state_; +/* ************************************************************************* */ +GaussNewtonOptimizer::GaussNewtonOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, + const GaussNewtonParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr(new State(initialValues, graph.error(initialValues)))), + params_(ensureHasOrdering(params, graph)) {} + +GaussNewtonOptimizer::GaussNewtonOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const Ordering& ordering) + : NonlinearOptimizer( + graph, std::unique_ptr(new State(initialValues, graph.error(initialValues)))) { + params_.ordering = ordering; +} + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr GaussNewtonOptimizer::iterate() { + gttic(GaussNewtonOptimizer_Iterate); // Linearize graph gttic(GaussNewtonOptimizer_Linearize); - GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values); + GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_->values); gttoc(GaussNewtonOptimizer_Linearize); // Solve Factor Graph gttic(GaussNewtonOptimizer_Solve); - const VectorValues delta = solve(*linear, current.values, params_); + const VectorValues delta = solve(*linear, params_); gttoc(GaussNewtonOptimizer_Solve); // Maybe show output - if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta"); + if (params_.verbosity >= NonlinearOptimizerParams::DELTA) + delta.print("delta"); // Create new state with new values and new error - state_.values = current.values.retract(delta); - state_.error = graph_.error(state_.values); - ++ state_.iterations; + Values newValues = state_->values.retract(delta); + state_.reset(new State(std::move(newValues), graph_.error(newValues), state_->iterations + 1)); + + return linear; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index 8b41a979c..3e75fc23d 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -30,14 +30,6 @@ class GaussNewtonOptimizer; class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams { }; -class GTSAM_EXPORT GaussNewtonState : public NonlinearOptimizerState { -protected: - GaussNewtonState(const NonlinearFactorGraph& graph, const Values& values, unsigned int iterations = 0) : - NonlinearOptimizerState(graph, values, iterations) {} - - friend class GaussNewtonOptimizer; -}; - /** * This class performs Gauss-Newton nonlinear optimization */ @@ -45,7 +37,6 @@ class GTSAM_EXPORT GaussNewtonOptimizer : public NonlinearOptimizer { protected: GaussNewtonParams params_; - GaussNewtonState state_; public: /// @name Standard interface @@ -59,9 +50,8 @@ public: * @param initialValues The initial variable assignments * @param params The optimization parameters */ - GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const GaussNewtonParams& params = GaussNewtonParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_(graph, initialValues) {} + GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const GaussNewtonParams& params = GaussNewtonParams()); /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -70,10 +60,8 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) : - NonlinearOptimizer(graph), state_(graph, initialValues) { - params_.ordering = ordering; } - + GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const Ordering& ordering); /// @} /// @name Advanced interface @@ -86,28 +74,16 @@ public: * containing the updated variable assignments, which may be retrieved with * values(). */ - virtual void iterate(); + GaussianFactorGraph::shared_ptr iterate() override; /** Read-only access the parameters */ const GaussNewtonParams& params() const { return params_; } - /** Read/write access the parameters. */ - GaussNewtonParams& params() { return params_; } - - /** Read-only access the last state */ - const GaussNewtonState& state() const { return state_; } - - /** Read/write access the last state. When modifying the state, the error, etc. must be consistent before calling iterate() */ - GaussNewtonState& state() { return state_; } - /// @} protected: /** Access the parameters (base class version) */ - virtual const NonlinearOptimizerParams& _params() const { return params_; } - - /** Access the state (base class version) */ - virtual const NonlinearOptimizerState& _state() const { return state_; } + const NonlinearOptimizerParams& _params() const override { return params_; } /** Internal function for computing a COLAMD ordering if no ordering is specified */ GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph) const; diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 0211df735..a0fc117d9 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -93,7 +93,7 @@ void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector(&relinearizeThreshold)) { - BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { + for(const VectorValues::KeyValuePair& key_delta: delta) { double maxDelta = key_delta.second.lpNorm(); if(maxDelta >= *threshold) relinKeys.insert(key_delta.first); @@ -120,7 +120,7 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } else if(const FastMap* thresholds = boost::get >(&relinearizeThreshold)) { - BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { + for(const VectorValues::KeyValuePair& key_delta: delta) { const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; if(threshold.rows() != key_delta.second.rows()) throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); @@ -138,7 +138,7 @@ void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, { // Check the current clique for relinearization bool relinearize = false; - BOOST_FOREACH(Key var, *clique->conditional()) { + for(Key var: *clique->conditional()) { double maxDelta = delta[var].lpNorm(); if(maxDelta >= threshold) { relinKeys.insert(var); @@ -148,7 +148,7 @@ void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, // If this node was relinearized, also check its children if(relinearize) { - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); } } @@ -161,7 +161,7 @@ void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMapconditional()) { + for(Key var: *clique->conditional()) { // Find the threshold for this variable type const Vector& threshold = thresholds.find(Symbol(var).chr())->second; @@ -180,7 +180,7 @@ void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMapchildren) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); } } @@ -192,7 +192,7 @@ KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector(relinearizeThreshold), delta, root); else if(relinearizeThreshold.type() == typeid(FastMap)) @@ -207,7 +207,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke static const bool debug = false; // does the separator contain any of the variables? bool found = false; - BOOST_FOREACH(Key key, clique->conditional()->parents()) { + for(Key key: clique->conditional()->parents()) { if (markedMask.exists(key)) { found = true; break; @@ -219,7 +219,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke if(debug) clique->print("Key(s) marked in clique "); if(debug) cout << "so marking key " << clique->conditional()->front() << endl; } - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { FindAll(child, keys, markedMask); } } @@ -267,7 +267,7 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, result.update(clique->conditional()->solve(result)); // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr& child, clique->children) + for(const boost::shared_ptr& child: clique->children) optimizeInPlace(child, result); } } @@ -280,14 +280,14 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - BOOST_FOREACH(const ISAM2::sharedClique& root, roots) + for(const ISAM2::sharedClique& root: roots) internal::optimizeInPlace(root, delta); lastBacksubVariableCount = delta.size(); } else { // Optimize with wildfire lastBacksubVariableCount = 0; - BOOST_FOREACH(const ISAM2::sharedClique& root, roots) + for(const ISAM2::sharedClique& root: roots) lastBacksubVariableCount += optimizeWildfireNonRecursive( root, wildfireThreshold, replacedKeys, delta); // modifies delta @@ -309,7 +309,7 @@ void updateRgProd(const boost::shared_ptr& clique, const KeySet& re // update deltas and recurse to children, but if not, we do not need to // recurse further because of the running separator property. bool anyReplaced = false; - BOOST_FOREACH(Key j, *clique->conditional()) { + for(Key j: *clique->conditional()) { if(replacedKeys.exists(j)) { anyReplaced = true; break; @@ -327,7 +327,7 @@ void updateRgProd(const boost::shared_ptr& clique, const KeySet& re // Write into RgProd vector DenseIndex vectorPosition = 0; - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { Vector& RgProdValue = RgProd[frontal]; RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); vectorPosition += RgProdValue.size(); @@ -339,7 +339,7 @@ void updateRgProd(const boost::shared_ptr& clique, const KeySet& re varsUpdated += clique->conditional()->nrFrontals(); // Recurse to children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } } } @@ -351,7 +351,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replac // Update variables size_t varsUpdated = 0; - BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { + for(const ISAM2::sharedClique& root: roots) { internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 538c66068..f04e16b7d 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -45,7 +45,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // Are any clique variables part of the tree that has been redone? bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { assert(cliqueReplaced == replaced.exists(frontal)); } #endif @@ -53,7 +53,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // If not redone, then has one of the separator variables changed significantly? bool recalculate = cliqueReplaced; if(!recalculate) { - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + for(Key parent: clique->conditional()->parents()) { if(changed.exists(parent)) { recalculate = true; break; @@ -94,7 +94,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // If the values were above the threshold or this clique was replaced if(valuesChanged) { // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { changed.insert(frontal); } } else { @@ -105,7 +105,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, } // Recurse to children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) { + for(const typename CLIQUE::shared_ptr& child: clique->children) { optimizeWildfire(child, threshold, changed, replaced, delta, count); } } @@ -122,7 +122,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Are any clique variables part of the tree that has been redone? bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { assert(cliqueReplaced == replaced.exists(frontal)); } #endif @@ -130,7 +130,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // If not redone, then has one of the separator variables changed significantly? bool recalculate = cliqueReplaced; if(!recalculate) { - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + for(Key parent: clique->conditional()->parents()) { if(changed.exists(parent)) { recalculate = true; break; @@ -140,6 +140,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Solve clique if it was replaced, or if any parents were changed above the // threshold or themselves replaced. + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, potentially refactor if(recalculate) { // Temporary copy of the original values, to check how much they change @@ -156,9 +157,9 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh boost::shared_ptr parent = clique->parent_.lock(); if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty())) { - BOOST_FOREACH(Key key, clique->conditional()->frontals()) + for(Key key: clique->conditional()->frontals()) clique->solnPointers_.insert(std::make_pair(key, delta.find(key))); - BOOST_FOREACH(Key key, clique->conditional()->parents()) + for(Key key: clique->conditional()->parents()) clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key))); } @@ -174,7 +175,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh DenseIndex dim = 0; FastVector parentPointers; parentPointers.reserve(clique->conditional()->nrParents()); - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + for(Key parent: clique->conditional()->parents()) { parentPointers.push_back(clique->solnPointers_.at(parent)); dim += parentPointers.back()->second.size(); } @@ -182,22 +183,27 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Fill parent vector xS.resize(dim); DenseIndex vectorPos = 0; - BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { + for(const VectorValues::const_iterator& parentPointer: parentPointers) { const Vector& parentVector = parentPointer->second; xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); vectorPos += parentVector.size(); } } - xS = c.getb() - c.get_S() * xS; - Vector soln = c.get_R().triangularView().solve(xS); + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + const Vector solution = c.get_R().triangularView().solve(rhs); // Check for indeterminant solution - if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); + if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); // Insert solution into a VectorValues DenseIndex vectorPosition = 0; for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { - clique->solnPointers_.at(*frontal)->second = soln.segment(vectorPosition, c.getDim(frontal)); + clique->solnPointers_.at(*frontal)->second = solution.segment(vectorPosition, c.getDim(frontal)); vectorPosition += c.getDim(frontal); } } @@ -227,7 +233,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // If the values were above the threshold or this clique was replaced if(valuesChanged) { // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { changed.insert(frontal); } } else { @@ -270,7 +276,7 @@ size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, doubl travStack.pop(); bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); if (recalculate) { - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children) { + for(const typename CLIQUE::shared_ptr& child: currentNode->children) { travStack.push(child); } } @@ -287,7 +293,7 @@ void nnz_internal(const boost::shared_ptr& clique, int& result) { int dimSep = (int)clique->conditional()->get_S().cols(); result += ((dimR+1)*dimR)/2 + dimSep*dimR; // traverse the children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) { + for(const typename CLIQUE::shared_ptr& child: clique->children) { nnz_internal(child, result); } } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d6f1a636a..18296b393 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -15,7 +15,6 @@ * @author Michael Kaess, Richard Roberts */ -#include #include // for operator += using namespace boost::assign; #include @@ -174,17 +173,17 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; - if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } } + if(debug) { for(const Key key: keys) { cout << key << " "; } } if(debug) cout << endl; NonlinearFactorGraph allAffected; KeySet indices; - BOOST_FOREACH(const Key key, keys) { + for(const Key key: keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); } if(debug) cout << "Affected factors are: "; - if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } + if(debug) { for(const size_t index: indices) { cout << index << " "; } } if(debug) cout << endl; return indices; } @@ -210,10 +209,10 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe gttic(check_candidates_and_linearize); GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); - BOOST_FOREACH(Key idx, candidates) { + for(Key idx: candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; - BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { + for(Key key: nonlinearFactors_[idx]->keys()) { if(affectedKeysSet.find(key) == affectedKeysSet.end()) { inside = false; break; @@ -251,7 +250,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { GaussianFactorGraph cachedBoundary; - BOOST_FOREACH(sharedClique orphan, orphans) { + for(sharedClique orphan: orphans) { // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); } @@ -292,10 +291,10 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke if(debug) { cout << "markedKeys: "; - BOOST_FOREACH(const Key key, markedKeys) { cout << key << " "; } + for(const Key key: markedKeys) { cout << key << " "; } cout << endl; cout << "observedKeys: "; - BOOST_FOREACH(const Key key, observedKeys) { cout << key << " "; } + for(const Key key: observedKeys) { cout << key << " "; } cout << endl; } @@ -322,7 +321,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke // ordering provides all keys in conditionals, there cannot be others because path to root included gttic(affectedKeys); FastList affectedKeys; - BOOST_FOREACH(const ConditionalType::shared_ptr& conditional, affectedBayesNet) + for(const ConditionalType::shared_ptr& conditional: affectedBayesNet) affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); @@ -349,7 +348,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke { // Only if some variables are unconstrained FastMap constraintGroups; - BOOST_FOREACH(Key var, observedKeys) + for(Key var: observedKeys) constraintGroups[var] = 1; order = Ordering::ColamdConstrained(variableIndex_, constraintGroups); } @@ -386,7 +385,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke // Reeliminated keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, theta_.keys()) { + for(Key key: theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; } } @@ -406,11 +405,11 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke if(debug) factors.print("Relinearized factors: "); gttoc(relinearizeAffected); - if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Key key, affectedKeys) { cout << key << " "; } cout << endl; } + if(debug) { cout << "Affected keys: "; for(const Key key: affectedKeys) { cout << key << " "; } cout << endl; } // Reeliminated keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, affectedAndNewKeys) { + for(Key key: affectedAndNewKeys) { result.detail->variableStatus[key].isReeliminated = true; } } @@ -437,7 +436,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(orphans); // Add the orphaned subtrees - BOOST_FOREACH(const sharedClique& orphan, orphans) + for(const sharedClique& orphan: orphans) factors += boost::make_shared >(orphan); gttoc(orphans); @@ -465,7 +464,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke } else { constraintGroups = FastMap(); const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - BOOST_FOREACH (Key var, observedKeys) + for (Key var: observedKeys) constraintGroups.insert(make_pair(var, group)); } @@ -500,8 +499,8 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke // Root clique variables for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(const sharedNode& root, this->roots()) - BOOST_FOREACH(Key var, *root->conditional()) + for(const sharedNode& root: this->roots()) + for(Key var: *root->conditional()) result.detail->variableStatus[var].inRootClique = true; } @@ -554,7 +553,7 @@ ISAM2Result ISAM2::update( // Remove the removed factors NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - BOOST_FOREACH(size_t index, removeFactorIndices) { + for(size_t index: removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if(params_.cacheLinearizedFactors) @@ -571,7 +570,7 @@ ISAM2Result ISAM2::update( // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. KeySet removedAndEmpty; - BOOST_FOREACH(Key key, removeFactors.keys()) { + for(Key key: removeFactors.keys()) { if(variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } @@ -580,7 +579,7 @@ ISAM2Result ISAM2::update( newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); // Get indices for unused keys - BOOST_FOREACH(Key key, unusedKeys) { + for(Key key: unusedKeys) { unusedIndices.insert(unusedIndices.end(), key); } } @@ -591,7 +590,7 @@ ISAM2Result ISAM2::update( Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); // New keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } + for(Key key: newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } gttoc(add_new_variables); gttic(evaluate_error_before); @@ -609,14 +608,14 @@ ISAM2Result ISAM2::update( } // Also mark any provided extra re-eliminate keys if(extraReelimKeys) { - BOOST_FOREACH(Key key, *extraReelimKeys) { + for(Key key: *extraReelimKeys) { markedKeys.insert(key); } } // Observed keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, markedKeys) { + for(Key key: markedKeys) { result.detail->variableStatus[key].isObserved = true; } } @@ -624,7 +623,7 @@ ISAM2Result ISAM2::update( // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Key value) instead of the iterator constructor. FastVector observedKeys; observedKeys.reserve(markedKeys.size()); - BOOST_FOREACH(Key index, markedKeys) { + for(Key index: markedKeys) { if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them } @@ -642,24 +641,24 @@ ISAM2Result ISAM2::update( if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging // Remove from relinKeys any keys whose linearization points are fixed - BOOST_FOREACH(Key key, fixedVariables_) { + for(Key key: fixedVariables_) { relinKeys.erase(key); } if(noRelinKeys) { - BOOST_FOREACH(Key key, *noRelinKeys) { + for(Key key: *noRelinKeys) { relinKeys.erase(key); } } // Above relin threshold keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, relinKeys) { + for(Key key: relinKeys) { result.detail->variableStatus[key].isAboveRelinThreshold = true; result.detail->variableStatus[key].isRelinearized = true; } } // Add the variables being relinearized to the marked keys KeySet markedRelinMask; - BOOST_FOREACH(const Key key, relinKeys) + for(const Key key: relinKeys) markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); gttoc(gather_relinearize_keys); @@ -667,16 +666,16 @@ ISAM2Result ISAM2::update( gttic(fluid_find_all); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. if (!relinKeys.empty()) { - BOOST_FOREACH(const sharedClique& root, roots_) + for(const sharedClique& root: roots_) // add other cliques that have the marked ones in the separator Impl::FindAll(root, markedKeys, markedRelinMask); // Relin involved keys for detailed results if(params_.enableDetailedResults) { KeySet involvedRelinKeys; - BOOST_FOREACH(const sharedClique& root, roots_) + for(const sharedClique& root: roots_) Impl::FindAll(root, involvedRelinKeys, markedRelinMask); - BOOST_FOREACH(Key key, involvedRelinKeys) { + for(Key key: involvedRelinKeys) { if(!result.detail->variableStatus[key].isAboveRelinThreshold) { result.detail->variableStatus[key].isRelinearizeInvolved = true; result.detail->variableStatus[key].isRelinearized = true; } } @@ -771,7 +770,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, KeySet leafKeysRemoved; // Remove each variable and its subtrees - BOOST_FOREACH(Key j, leafKeys) { + for(Key j: leafKeys) { if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree // Traverse up the tree to find the root of the marginalized subtree @@ -789,7 +788,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // See if we should remove the whole clique bool marginalizeEntireClique = true; - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { if(!leafKeys.exists(frontal)) { marginalizeEntireClique = false; break; } } @@ -806,10 +805,10 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques - BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { + for(const sharedClique& removedClique: removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) + for(Key frontal: removedClique->conditional()->frontals()) { // Add to factors to remove const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; @@ -832,9 +831,9 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, GaussianFactorGraph graph; KeySet factorsInSubtreeRoot; Cliques subtreesToRemove; - BOOST_FOREACH(const sharedClique& child, clique->children) { + for(const sharedClique& child: clique->children) { // Remove subtree if child depends on any marginalized keys - BOOST_FOREACH(Key parent, child->conditional()->parents()) { + for(Key parent: child->conditional()->parents()) { if(leafKeys.exists(parent)) { subtreesToRemove.push_back(child); graph.push_back(child->cachedFactor()); // Add child marginal @@ -843,13 +842,13 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, } } Cliques childrenRemoved; - BOOST_FOREACH(const sharedClique& childToRemove, subtreesToRemove) { + for(const sharedClique& childToRemove: subtreesToRemove) { const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); - BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { + for(const sharedClique& removedClique: removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) + for(Key frontal: removedClique->conditional()->frontals()) { // Add to factors to remove const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; @@ -867,16 +866,16 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // but do not involve frontal variables of any of its children. // TODO: reuse cached linear factors KeySet factorsFromMarginalizedInClique_step1; - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { if(leafKeys.exists(frontal)) factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } // Remove any factors in subtrees that we're removing at this step - BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { - BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) { - BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) { + for(const sharedClique& removedChild: childrenRemoved) { + for(Key indexInClique: removedChild->conditional()->frontals()) { + for(Key factorInvolving: variableIndex_[indexInClique]) { factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } // Create factor graph from factor indices - BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) { + for(size_t i: factorsFromMarginalizedInClique_step1) { graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the conditional @@ -908,7 +907,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, clique->conditional()->nrFrontals() -= nToRemove; // Add to factors to remove factors involved in frontals of current clique - BOOST_FOREACH(Key frontal, cliqueFrontalsToEliminate) + for(Key frontal: cliqueFrontalsToEliminate) { const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); @@ -925,8 +924,8 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // Gather factors to add - the new marginal factors GaussianFactorGraph factorsToAdd; typedef pair > Key_Factors; - BOOST_FOREACH(const Key_Factors& key_factors, marginalFactors) { - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, key_factors.second) { + for(const Key_Factors& key_factors: marginalFactors) { + for(const GaussianFactor::shared_ptr& factor: key_factors.second) { if(factor) { factorsToAdd.push_back(factor); if(marginalFactorsIndices) @@ -935,7 +934,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, factor)); if(params_.cacheLinearizedFactors) linearFactors_.push_back(factor); - BOOST_FOREACH(Key factorKey, *factor) { + for(Key factorKey: *factor) { fixedVariables_.insert(factorKey); } } } @@ -944,7 +943,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // Remove the factors to remove that have been summarized in the newly-added marginal factors NonlinearFactorGraph removedFactors; - BOOST_FOREACH(size_t i, factorIndicesToRemove) { + for(size_t i: factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); if(params_.cacheLinearizedFactors) @@ -1005,7 +1004,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const gttic(Copy_dx_d); // Update Delta and linear step - doglegDelta_ = doglegResult.Delta; + doglegDelta_ = doglegResult.delta; delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); } @@ -1065,7 +1064,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, // Recursively add contributions from children typedef boost::shared_ptr sharedClique; - BOOST_FOREACH(const sharedClique& child, root->children) { + for(const sharedClique& child: root->children) { gradientAtZeroTreeAdder(child, g); } } @@ -1077,7 +1076,7 @@ VectorValues ISAM2::gradientAtZero() const VectorValues g; // Sum up contributions for each clique - BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots()) + for(const ISAM2::sharedClique& root: this->roots()) gradientAtZeroTreeAdder(root, g); return g; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 27f21be12..114c04018 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -200,7 +200,7 @@ struct GTSAM_EXPORT ISAM2Params { else { std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; - BOOST_FOREACH(const ISAM2ThresholdMapValue& value, boost::get(relinearizeThreshold)) { + for(const ISAM2ThresholdMapValue& value: boost::get(relinearizeThreshold)) { std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; } } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 9e42afa33..5f29c3bdf 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -11,392 +11,286 @@ /** * @file LevenbergMarquardtOptimizer.cpp - * @brief + * @brief A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme * @author Richard Roberts + * @author Frank Dellaert * @author Luca Carlone - * @date Feb 26, 2012 + * @date Feb 26, 2012 */ #include -#include +#include +#include +#include #include -#include -#include +#include +#include +#include #include -#include -#include #include +#include +#include +#include #include +#include #include #include -#include using namespace std; namespace gtsam { using boost::adaptors::map_values; +typedef internal::LevenbergMarquardtState State; /* ************************************************************************* */ -LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator( - const std::string &src) { - std::string s = src; - boost::algorithm::to_upper(s); - if (s == "SILENT") - return LevenbergMarquardtParams::SILENT; - if (s == "SUMMARY") - return LevenbergMarquardtParams::SUMMARY; - if (s == "LAMBDA") - return LevenbergMarquardtParams::LAMBDA; - if (s == "TRYLAMBDA") - return LevenbergMarquardtParams::TRYLAMBDA; - if (s == "TRYCONFIG") - return LevenbergMarquardtParams::TRYCONFIG; - if (s == "TRYDELTA") - return LevenbergMarquardtParams::TRYDELTA; - if (s == "DAMPED") - return LevenbergMarquardtParams::DAMPED; +LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, + const LevenbergMarquardtParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr(new State(initialValues, graph.error(initialValues), + params.lambdaInitial, params.lambdaFactor))), + params_(LevenbergMarquardtParams::EnsureHasOrdering(params, graph)) {} - /* default is silent */ - return LevenbergMarquardtParams::SILENT; +LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr(new State(initialValues, graph.error(initialValues), + params.lambdaInitial, params.lambdaFactor))), + params_(LevenbergMarquardtParams::ReplaceOrdering(params, ordering)) {} + +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::initTime() { + startTime_ = boost::posix_time::microsec_clock::universal_time(); } /* ************************************************************************* */ -std::string LevenbergMarquardtParams::verbosityLMTranslator( - VerbosityLM value) { - std::string s; - switch (value) { - case LevenbergMarquardtParams::SILENT: - s = "SILENT"; - break; - case LevenbergMarquardtParams::SUMMARY: - s = "SUMMARY"; - break; - case LevenbergMarquardtParams::TERMINATION: - s = "TERMINATION"; - break; - case LevenbergMarquardtParams::LAMBDA: - s = "LAMBDA"; - break; - case LevenbergMarquardtParams::TRYLAMBDA: - s = "TRYLAMBDA"; - break; - case LevenbergMarquardtParams::TRYCONFIG: - s = "TRYCONFIG"; - break; - case LevenbergMarquardtParams::TRYDELTA: - s = "TRYDELTA"; - break; - case LevenbergMarquardtParams::DAMPED: - s = "DAMPED"; - break; - default: - s = "UNDEFINED"; - break; - } - return s; +double LevenbergMarquardtOptimizer::lambda() const { + auto currentState = static_cast(state_.get()); + return currentState->lambda; } /* ************************************************************************* */ -void LevenbergMarquardtParams::print(const std::string& str) const { - NonlinearOptimizerParams::print(str); - std::cout << " lambdaInitial: " << lambdaInitial << "\n"; - std::cout << " lambdaFactor: " << lambdaFactor << "\n"; - std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; - std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; - std::cout << " minModelFidelity: " << minModelFidelity << "\n"; - std::cout << " diagonalDamping: " << diagonalDamping << "\n"; - std::cout << " minDiagonal: " << minDiagonal << "\n"; - std::cout << " maxDiagonal: " << maxDiagonal << "\n"; - std::cout << " verbosityLM: " - << verbosityLMTranslator(verbosityLM) << "\n"; - std::cout.flush(); +int LevenbergMarquardtOptimizer::getInnerIterations() const { + auto currentState = static_cast(state_.get()); + return currentState->totalNumberInnerIterations; } /* ************************************************************************* */ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { - return graph_.linearize(state_.values); + return graph_.linearize(state_->values); } /* ************************************************************************* */ -void LevenbergMarquardtOptimizer::increaseLambda() { - if (params_.useFixedLambdaFactor) { - state_.lambda *= params_.lambdaFactor; - } else { - state_.lambda *= params_.lambdaFactor; - params_.lambdaFactor *= 2.0; - } - state_.reuseDiagonal = true; -} - -/* ************************************************************************* */ -void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { - - if (params_.useFixedLambdaFactor) { - state_.lambda /= params_.lambdaFactor; - } else { - // CHECK_GT(step_quality, 0.0); - state_.lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); - params_.lambdaFactor = 2.0; - } - state_.lambda = std::max(params_.lambdaLowerBound, state_.lambda); - state_.reuseDiagonal = false; - -} - -/* ************************************************************************* */ -GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( - const GaussianFactorGraph& linear) { - +GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( + const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal) const { gttic(damp); + auto currentState = static_cast(state_.get()); + if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) - cout << "building damped system with lambda " << state_.lambda << endl; + std::cout << "building damped system with lambda " << currentState->lambda << std::endl; - // Only retrieve diagonal vector when reuse_diagonal = false - if (params_.diagonalDamping && state_.reuseDiagonal == false) { - state_.hessianDiagonal = linear.hessianDiagonal(); - BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { - for (int aa = 0; aa < v.size(); aa++) { - v(aa) = std::min(std::max(v(aa), params_.minDiagonal), - params_.maxDiagonal); - v(aa) = sqrt(v(aa)); - } - } - } // reuse diagonal - - // for each of the variables, add a prior - double sigma = 1.0 / std::sqrt(state_.lambda); - GaussianFactorGraph::shared_ptr dampedPtr = linear.cloneToPtr(); - GaussianFactorGraph &damped = (*dampedPtr); - damped.reserve(damped.size() + state_.values.size()); - if (params_.diagonalDamping) { - BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) { - // Fill in the diagonal of A with diag(hessian) - try { - Matrix A = Eigen::DiagonalMatrix( - state_.hessianDiagonal.at(key_vector.first)); - size_t dim = key_vector.second.size(); - Vector b = Vector::Zero(dim); - SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - damped += boost::make_shared(key_vector.first, A, b, - model); - } catch (const std::exception& e) { - // Don't attempt any damping if no key found in diagonal - continue; - } - } - } else { - // Straightforward damping: - - // initialize noise model cache to a reasonable default size - NoiseCacheVector noises(6); - BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { - size_t dim = key_value.value.dim(); - - if (dim > noises.size()) - noises.resize(dim); - - NoiseCacheItem& item = noises[dim-1]; - - // Initialize noise model, A and b if we haven't done so already - if(!item.model) { - item.A = Matrix::Identity(dim, dim); - item.b = Vector::Zero(dim); - item.model = noiseModel::Isotropic::Sigma(dim, sigma); - } - damped += boost::make_shared(key_value.key, item.A, item.b, item.model); - } - } - gttoc(damp); - return dampedPtr; + if (params_.diagonalDamping) + return currentState->buildDampedSystem(linear, sqrtHessianDiagonal); + else + return currentState->buildDampedSystem(linear); } /* ************************************************************************* */ // Log current error/lambda to file inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){ + auto currentState = static_cast(state_.get()); + if (!params_.logFile.empty()) { ofstream os(params_.logFile.c_str(), ios::app); boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); - os << /*inner iterations*/ state_.totalNumberInnerIterations << "," - << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," - << /*current error*/ currentError << "," << state_.lambda << "," - << /*outer iterations*/ state_.iterations << endl; + os << /*inner iterations*/ currentState->totalNumberInnerIterations << "," + << 1e-6 * (currentTime - startTime_).total_microseconds() << "," + << /*current error*/ currentError << "," << currentState->lambda << "," + << /*outer iterations*/ currentState->iterations << endl; } } /* ************************************************************************* */ -void LevenbergMarquardtOptimizer::iterate() { +bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, + const VectorValues& sqrtHessianDiagonal) { + auto currentState = static_cast(state_.get()); + bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA); + +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + boost::timer::cpu_timer lamda_iteration_timer; + lamda_iteration_timer.start(); +#else + boost::timer lamda_iteration_timer; + lamda_iteration_timer.restart(); +#endif + + if (verbose) + cout << "trying lambda = " << currentState->lambda << endl; + + // Build damped system for this lambda (adds prior factors that make it like gradient descent) + auto dampedSystem = buildDampedSystem(linear, sqrtHessianDiagonal); + + // Try solving + double modelFidelity = 0.0; + bool step_is_successful = false; + bool stopSearchingLambda = false; + double newError = numeric_limits::infinity(), costChange; + Values newValues; + VectorValues delta; + + bool systemSolvedSuccessfully; + try { + // ============ Solve is where most computation happens !! ================= + delta = solve(dampedSystem, params_); + systemSolvedSuccessfully = true; + } catch (const IndeterminantLinearSystemException& e) { + systemSolvedSuccessfully = false; + } + + if (systemSolvedSuccessfully) { + if (verbose) + cout << "linear delta norm = " << delta.norm() << endl; + if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA) + delta.print("delta"); + + // cost change in the linearized system (old - new) + double newlinearizedError = linear.error(delta); + + double linearizedCostChange = currentState->error - newlinearizedError; + if (verbose) + cout << "newlinearizedError = " << newlinearizedError + << " linearizedCostChange = " << linearizedCostChange << endl; + + if (linearizedCostChange >= 0) { // step is valid + // update values + gttic(retract); + // ============ This is where the solution is updated ==================== + newValues = currentState->values.retract(delta); + // ======================================================================= + gttoc(retract); + + // compute new error + gttic(compute_error); + if (verbose) + cout << "calculating error:" << endl; + newError = graph_.error(newValues); + gttoc(compute_error); + + if (verbose) + cout << "old error (" << currentState->error << ") new (tentative) error (" << newError + << ")" << endl; + + // cost change in the original, nonlinear system (old - new) + costChange = currentState->error - newError; + + if (linearizedCostChange > + 1e-20) { // the (linear) error has to decrease to satisfy this condition + // fidelity of linearized model VS original system between + modelFidelity = costChange / linearizedCostChange; + // if we decrease the error in the nonlinear system and modelFidelity is above threshold + step_is_successful = modelFidelity > params_.minModelFidelity; + if (verbose) + cout << "modelFidelity: " << modelFidelity << endl; + } // else we consider the step non successful and we either increase lambda or stop if error + // change is small + + double minAbsoluteTolerance = params_.relativeErrorTol * currentState->error; + // if the change is small we terminate + if (fabs(costChange) < minAbsoluteTolerance) { + if (verbose) + cout << "fabs(costChange)=" << fabs(costChange) + << " minAbsoluteTolerance=" << minAbsoluteTolerance + << " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl; + stopSearchingLambda = true; + } + } + } // if (systemSolvedSuccessfully) + + if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { +// do timing +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; +#else + double iterationTime = lamda_iteration_timer.elapsed(); +#endif + if (currentState->iterations == 0) + cout << "iter cost cost_change lambda success iter_time" << endl; + + cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d % 3.2e") % currentState->iterations % + newError % costChange % currentState->lambda % systemSolvedSuccessfully % + iterationTime << endl; + } + + if (step_is_successful) { + // we have successfully decreased the cost and we have good modelFidelity + // NOTE(frank): As we return immediately after this, we move the newValues + // TODO(frank): make Values actually support move. Does not seem to happen now. + state_ = currentState->decreaseLambda(params_, modelFidelity, std::move(newValues), newError); + return true; + } else if (!stopSearchingLambda) { // we failed to solved the system or had no decrease in cost + if (verbose) + cout << "increasing lambda" << endl; + State* modifiedState = static_cast(state_.get()); + modifiedState->increaseLambda(params_); // TODO(frank): make this functional with Values move + + // check if lambda is too big + if (modifiedState->lambda >= params_.lambdaUpperBound) { + if (params_.verbosity >= NonlinearOptimizerParams::TERMINATION || + params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) + cout << "Warning: Levenberg-Marquardt giving up because " + "cannot decrease error with maximum lambda" << endl; + return true; + } else { + return false; // only case where we will keep trying + } + } else { // the change in the cost is very small and it is not worth trying bigger lambdas + if (verbose) + cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl; + return true; + } +} + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::iterate() { + auto currentState = static_cast(state_.get()); gttic(LM_iterate); - // Pull out parameters we'll use - const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; - const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM; - // Linearize graph - if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) + if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); - if(state_.totalNumberInnerIterations==0) { // write initial error - writeLogFile(state_.error); + if(currentState->totalNumberInnerIterations==0) { // write initial error + writeLogFile(currentState->error); - if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { - cout << "Initial error: " << state_.error << ", values: " << state_.values.size() - << std::endl; + if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + cout << "Initial error: " << currentState->error + << ", values: " << currentState->values.size() << std::endl; + } + } + + // Only calculate diagonal of Hessian (expensive) once per outer iteration, if we need it + VectorValues sqrtHessianDiagonal; + if (params_.diagonalDamping) { + sqrtHessianDiagonal = linear->hessianDiagonal(); + for (Vector& v : sqrtHessianDiagonal | map_values) { + v = v.cwiseMax(params_.minDiagonal).cwiseMin(params_.maxDiagonal).cwiseSqrt(); } } // Keep increasing lambda until we make make progress - while (true) { + while (!tryLambda(*linear, sqrtHessianDiagonal)) { + auto newState = static_cast(state_.get()); + writeLogFile(newState->error); + } -#ifdef GTSAM_USING_NEW_BOOST_TIMERS - boost::timer::cpu_timer lamda_iteration_timer; - lamda_iteration_timer.start(); -#else - boost::timer lamda_iteration_timer; - lamda_iteration_timer.restart(); -#endif - - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "trying lambda = " << state_.lambda << endl; - - // Build damped system for this lambda (adds prior factors that make it like gradient descent) - GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear); - GaussianFactorGraph &dampedSystem = (*dampedSystemPtr); - - // Try solving - double modelFidelity = 0.0; - bool step_is_successful = false; - bool stopSearchingLambda = false; - double newError = numeric_limits::infinity(), costChange; - Values newValues; - VectorValues delta; - - bool systemSolvedSuccessfully; - try { - // ============ Solve is where most computation happens !! ================= - delta = solve(dampedSystem, state_.values, params_); - systemSolvedSuccessfully = true; - } catch (const IndeterminantLinearSystemException& e) { - systemSolvedSuccessfully = false; - } - - if (systemSolvedSuccessfully) { - state_.reuseDiagonal = true; - - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "linear delta norm = " << delta.norm() << endl; - if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) - delta.print("delta"); - - // cost change in the linearized system (old - new) - double newlinearizedError = linear->error(delta); - - double linearizedCostChange = state_.error - newlinearizedError; - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "newlinearizedError = " << newlinearizedError << - " linearizedCostChange = " << linearizedCostChange << endl; - - if (linearizedCostChange >= 0) { // step is valid - // update values - gttic(retract); - // ============ This is where the solution is updated ==================== - newValues = state_.values.retract(delta); - // ======================================================================= - gttoc(retract); - - // compute new error - gttic(compute_error); - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "calculating error:" << endl; - newError = graph_.error(newValues); - gttoc(compute_error); - - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "old error (" << state_.error - << ") new (tentative) error (" << newError << ")" << endl; - - // cost change in the original, nonlinear system (old - new) - costChange = state_.error - newError; - - if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition - // fidelity of linearized model VS original system between - modelFidelity = costChange / linearizedCostChange; - // if we decrease the error in the nonlinear system and modelFidelity is above threshold - step_is_successful = modelFidelity > params_.minModelFidelity; - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "modelFidelity: " << modelFidelity << endl; - } // else we consider the step non successful and we either increase lambda or stop if error change is small - - double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; - // if the change is small we terminate - if (fabs(costChange) < minAbsoluteTolerance){ - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "fabs(costChange)="<= LevenbergMarquardtParams::TRYLAMBDA) - cout << "increasing lambda" << endl; - increaseLambda(); - writeLogFile(state_.error); - - // check if lambda is too big - if (state_.lambda >= params_.lambdaUpperBound) { - if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION || - lmVerbosity == LevenbergMarquardtParams::SUMMARY) - cout << "Warning: Levenberg-Marquardt giving up because " - "cannot decrease error with maximum lambda" << endl; - break; - } - } else { // the change in the cost is very small and it is not worth trying bigger lambdas - writeLogFile(state_.error); - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl; - break; - } - } // end while - - // Increment the iteration counter - ++state_.iterations; -} - -/* ************************************************************************* */ -LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( - LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { - if (!params.ordering) - params.ordering = Ordering::Create(params.orderingType, graph); - return params; + return linear; } } /* namespace gtsam */ diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 2be4a218e..f1135d2f0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -11,14 +11,17 @@ /** * @file LevenbergMarquardtOptimizer.h - * @brief + * @brief A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme * @author Richard Roberts - * @date Feb 26, 2012 + * @author Frank Dellaert + * @author Luca Carlone + * @date Feb 26, 2012 */ #pragma once #include +#include #include #include @@ -26,158 +29,21 @@ class NonlinearOptimizerMoreOptimizationTest; namespace gtsam { -class LevenbergMarquardtOptimizer; - -/** Parameters for Levenberg-Marquardt optimization. Note that this parameters - * class inherits from NonlinearOptimizerParams, which specifies the parameters - * common to all nonlinear optimization algorithms. This class also contains - * all of those parameters. - */ -class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { - -public: - /** See LevenbergMarquardtParams::lmVerbosity */ - enum VerbosityLM { - SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA - }; - - static VerbosityLM verbosityLMTranslator(const std::string &s); - static std::string verbosityLMTranslator(VerbosityLM value); - -public: - - double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-5) - double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0) - double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) - double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) - VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity - double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration - std::string logFile; ///< an optional CSV log file, with [iteration, time, error, lambda] - bool diagonalDamping; ///< if true, use diagonal of Hessian - bool useFixedLambdaFactor; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor - double minDiagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) - double maxDiagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) - - LevenbergMarquardtParams() - : verbosityLM(SILENT), - diagonalDamping(false), - minDiagonal(1e-6), - maxDiagonal(1e32) { - SetLegacyDefaults(this); - } - - static void SetLegacyDefaults(LevenbergMarquardtParams* p) { - // Relevant NonlinearOptimizerParams: - p->maxIterations = 100; - p->relativeErrorTol = 1e-5; - p->absoluteErrorTol = 1e-5; - // LM-specific: - p->lambdaInitial = 1e-5; - p->lambdaFactor = 10.0; - p->lambdaUpperBound = 1e5; - p->lambdaLowerBound = 0.0; - p->minModelFidelity = 1e-3; - p->diagonalDamping = false; - p->useFixedLambdaFactor = true; - } - - // these do seem to work better for SFM - static void SetCeresDefaults(LevenbergMarquardtParams* p) { - // Relevant NonlinearOptimizerParams: - p->maxIterations = 50; - p->absoluteErrorTol = 0; // No corresponding option in CERES - p->relativeErrorTol = 1e-6; // This is function_tolerance - // LM-specific: - p->lambdaUpperBound = 1e32; - p->lambdaLowerBound = 1e-16; - p->lambdaInitial = 1e-04; - p->lambdaFactor = 2.0; - p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES - p->diagonalDamping = true; - p->useFixedLambdaFactor = false; // This is important - } - - static LevenbergMarquardtParams LegacyDefaults() { - LevenbergMarquardtParams p; - SetLegacyDefaults(&p); - return p; - } - - static LevenbergMarquardtParams CeresDefaults() { - LevenbergMarquardtParams p; - SetCeresDefaults(&p); - return p; - } - - virtual ~LevenbergMarquardtParams() {} - virtual void print(const std::string& str = "") const; - - /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. - /// @{ - bool getDiagonalDamping() const { return diagonalDamping; } - double getlambdaFactor() const { return lambdaFactor; } - double getlambdaInitial() const { return lambdaInitial; } - double getlambdaLowerBound() const { return lambdaLowerBound; } - double getlambdaUpperBound() const { return lambdaUpperBound; } - std::string getLogFile() const { return logFile; } - std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} - void setDiagonalDamping(bool flag) { diagonalDamping = flag; } - void setlambdaFactor(double value) { lambdaFactor = value; } - void setlambdaInitial(double value) { lambdaInitial = value; } - void setlambdaLowerBound(double value) { lambdaLowerBound = value; } - void setlambdaUpperBound(double value) { lambdaUpperBound = value; } - void setLogFile(const std::string& s) { logFile = s; } - void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} - void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} - // @} -}; - -/** - * State for LevenbergMarquardtOptimizer - */ -class GTSAM_EXPORT LevenbergMarquardtState: public NonlinearOptimizerState { - -public: - double lambda; - boost::posix_time::ptime startTime; - int totalNumberInnerIterations; //< The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) - VectorValues hessianDiagonal; //< we only update hessianDiagonal when reuseDiagonal = false - bool reuseDiagonal; ///< an additional option in Ceres for diagonalDamping - - LevenbergMarquardtState() {} // used in LM constructor but immediately overwritten - - void initTime() { - startTime = boost::posix_time::microsec_clock::universal_time(); - } - - virtual ~LevenbergMarquardtState() { - } - -protected: - LevenbergMarquardtState(const NonlinearFactorGraph& graph, - const Values& initialValues, const LevenbergMarquardtParams& params, - unsigned int iterations = 0) : - NonlinearOptimizerState(graph, initialValues, iterations), lambda( - params.lambdaInitial), totalNumberInnerIterations(0),reuseDiagonal(false) { - initTime(); - } - - friend class LevenbergMarquardtOptimizer; -}; - /** * This class performs Levenberg-Marquardt nonlinear optimization */ class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer { protected: - LevenbergMarquardtParams params_; ///< LM parameters - LevenbergMarquardtState state_; ///< optimization state + const LevenbergMarquardtParams params_; ///< LM parameters + boost::posix_time::ptime startTime_; + + void initTime(); public: typedef boost::shared_ptr shared_ptr; - /// @name Standard interface + /// @name Constructors/Destructor /// @{ /** Standard constructor, requires a nonlinear factor graph, initial @@ -188,12 +54,8 @@ public: * @param initialValues The initial variable assignments * @param params The optimization parameters */ - LevenbergMarquardtOptimizer( - const NonlinearFactorGraph& graph, const Values& initialValues, - const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) - : NonlinearOptimizer(graph), - params_(ensureHasOrdering(params, graph)), - state_(graph, initialValues, params_) {} + LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()); /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -202,33 +64,27 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - LevenbergMarquardtOptimizer( - const NonlinearFactorGraph& graph, const Values& initialValues, - const Ordering& ordering, - const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) - : NonlinearOptimizer(graph), params_(params) { - params_.ordering = ordering; - state_ = LevenbergMarquardtState(graph, initialValues, params_); + LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()); + + /** Virtual destructor */ + virtual ~LevenbergMarquardtOptimizer() { } + /// @} + + /// @name Standard interface + /// @{ + /// Access the current damping value - double lambda() const { - return state_.lambda; - } - - // Apply policy to increase lambda if the current update was successful (stepQuality not used in the naive policy) - void increaseLambda(); - - // Apply policy to decrease lambda if the current update was NOT successful (stepQuality not used in the naive policy) - void decreaseLambda(double stepQuality); + double lambda() const; /// Access the current number of inner iterations - int getInnerIterations() const { - return state_.totalNumberInnerIterations; - } + int getInnerIterations() const; /// print - virtual void print(const std::string& str = "") const { + void print(const std::string& str = "") const { std::cout << str << "LevenbergMarquardtOptimizer" << std::endl; this->params_.print(" parameters:\n"); } @@ -238,73 +94,37 @@ public: /// @name Advanced interface /// @{ - /** Virtual destructor */ - virtual ~LevenbergMarquardtOptimizer() { - } - /** Perform a single iteration, returning a new NonlinearOptimizer class * containing the updated variable assignments, which may be retrieved with * values(). */ - virtual void iterate(); + GaussianFactorGraph::shared_ptr iterate() override; /** Read-only access the parameters */ const LevenbergMarquardtParams& params() const { return params_; } - /** Read/write access the parameters */ - LevenbergMarquardtParams& params() { - return params_; - } - - /** Read-only access the last state */ - const LevenbergMarquardtState& state() const { - return state_; - } - - /** Read/write access the last state. When modifying the state, the error, etc. must be consistent before calling iterate() */ - LevenbergMarquardtState& state() { - return state_; - } - - /** Build a damped system for a specific lambda */ - GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear); - friend class ::NonlinearOptimizerMoreOptimizationTest; - - /** Small struct to cache objects needed for damping. - * This is used in buildDampedSystem */ - struct NoiseCacheItem { - Matrix A; - Vector b; - SharedDiagonal model; - }; - - /// Noise model Cache - typedef std::vector NoiseCacheVector; - void writeLogFile(double currentError); + /** linearize, can be overwritten */ + virtual GaussianFactorGraph::shared_ptr linearize() const; + + /** Build a damped system for a specific lambda -- for testing only */ + GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear, + const VectorValues& sqrtHessianDiagonal) const; + + /** Inner loop, changes state, returns true if successful or giving up */ + bool tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal); + /// @} protected: /** Access the parameters (base class version) */ - virtual const NonlinearOptimizerParams& _params() const { + const NonlinearOptimizerParams& _params() const override { return params_; } - - /** Access the state (base class version) */ - virtual const NonlinearOptimizerState& _state() const { - return state_; - } - - /** Internal function for computing a COLAMD ordering if no ordering is specified */ - LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params, - const NonlinearFactorGraph& graph) const; - - /** linearize, can be overwritten */ - virtual GaussianFactorGraph::shared_ptr linearize() const; }; } diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.cpp b/gtsam/nonlinear/LevenbergMarquardtParams.cpp new file mode 100644 index 000000000..5d558a43a --- /dev/null +++ b/gtsam/nonlinear/LevenbergMarquardtParams.cpp @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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 LevenbergMarquardtParams.cpp + * @brief Parameters for Levenberg-Marquardt trust-region scheme + * @author Richard Roberts + * @author Frank Dellaert + * @author Luca Carlone + * @date Feb 26, 2012 + */ + +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator( + const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return LevenbergMarquardtParams::SILENT; + if (s == "SUMMARY") + return LevenbergMarquardtParams::SUMMARY; + if (s == "LAMBDA") + return LevenbergMarquardtParams::LAMBDA; + if (s == "TRYLAMBDA") + return LevenbergMarquardtParams::TRYLAMBDA; + if (s == "TRYCONFIG") + return LevenbergMarquardtParams::TRYCONFIG; + if (s == "TRYDELTA") + return LevenbergMarquardtParams::TRYDELTA; + if (s == "DAMPED") + return LevenbergMarquardtParams::DAMPED; + + /* default is silent */ + return LevenbergMarquardtParams::SILENT; +} + +/* ************************************************************************* */ +std::string LevenbergMarquardtParams::verbosityLMTranslator( + VerbosityLM value) { + std::string s; + switch (value) { + case LevenbergMarquardtParams::SILENT: + s = "SILENT"; + break; + case LevenbergMarquardtParams::SUMMARY: + s = "SUMMARY"; + break; + case LevenbergMarquardtParams::TERMINATION: + s = "TERMINATION"; + break; + case LevenbergMarquardtParams::LAMBDA: + s = "LAMBDA"; + break; + case LevenbergMarquardtParams::TRYLAMBDA: + s = "TRYLAMBDA"; + break; + case LevenbergMarquardtParams::TRYCONFIG: + s = "TRYCONFIG"; + break; + case LevenbergMarquardtParams::TRYDELTA: + s = "TRYDELTA"; + break; + case LevenbergMarquardtParams::DAMPED: + s = "DAMPED"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +/* ************************************************************************* */ +void LevenbergMarquardtParams::print(const std::string& str) const { + NonlinearOptimizerParams::print(str); + std::cout << " lambdaInitial: " << lambdaInitial << "\n"; + std::cout << " lambdaFactor: " << lambdaFactor << "\n"; + std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; + std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; + std::cout << " minModelFidelity: " << minModelFidelity << "\n"; + std::cout << " diagonalDamping: " << diagonalDamping << "\n"; + std::cout << " minDiagonal: " << minDiagonal << "\n"; + std::cout << " maxDiagonal: " << maxDiagonal << "\n"; + std::cout << " verbosityLM: " + << verbosityLMTranslator(verbosityLM) << "\n"; + std::cout.flush(); +} + +} /* namespace gtsam */ + diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h new file mode 100644 index 000000000..abb8c3c22 --- /dev/null +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * 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 LevenbergMarquardtParams.h + * @brief Parameters for Levenberg-Marquardt trust-region scheme + * @author Richard Roberts + * @author Frank Dellaert + * @author Luca Carlone + * @date Feb 26, 2012 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** Parameters for Levenberg-Marquardt optimization. Note that this parameters + * class inherits from NonlinearOptimizerParams, which specifies the parameters + * common to all nonlinear optimization algorithms. This class also contains + * all of those parameters. + */ +class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { + +public: + /** See LevenbergMarquardtParams::lmVerbosity */ + enum VerbosityLM { + SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA + }; + + static VerbosityLM verbosityLMTranslator(const std::string &s); + static std::string verbosityLMTranslator(VerbosityLM value); + +public: + + double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-5) + double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0) + double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) + double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) + VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity + double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration + std::string logFile; ///< an optional CSV log file, with [iteration, time, error, lambda] + bool diagonalDamping; ///< if true, use diagonal of Hessian + bool useFixedLambdaFactor; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor + double minDiagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) + double maxDiagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) + + LevenbergMarquardtParams() + : verbosityLM(SILENT), + diagonalDamping(false), + minDiagonal(1e-6), + maxDiagonal(1e32) { + SetLegacyDefaults(this); + } + + static void SetLegacyDefaults(LevenbergMarquardtParams* p) { + // Relevant NonlinearOptimizerParams: + p->maxIterations = 100; + p->relativeErrorTol = 1e-5; + p->absoluteErrorTol = 1e-5; + // LM-specific: + p->lambdaInitial = 1e-5; + p->lambdaFactor = 10.0; + p->lambdaUpperBound = 1e5; + p->lambdaLowerBound = 0.0; + p->minModelFidelity = 1e-3; + p->diagonalDamping = false; + p->useFixedLambdaFactor = true; + } + + // these do seem to work better for SFM + static void SetCeresDefaults(LevenbergMarquardtParams* p) { + // Relevant NonlinearOptimizerParams: + p->maxIterations = 50; + p->absoluteErrorTol = 0; // No corresponding option in CERES + p->relativeErrorTol = 1e-6; // This is function_tolerance + // LM-specific: + p->lambdaUpperBound = 1e32; + p->lambdaLowerBound = 1e-16; + p->lambdaInitial = 1e-04; + p->lambdaFactor = 2.0; + p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES + p->diagonalDamping = true; + p->useFixedLambdaFactor = false; // This is important + } + + static LevenbergMarquardtParams LegacyDefaults() { + LevenbergMarquardtParams p; + SetLegacyDefaults(&p); + return p; + } + + static LevenbergMarquardtParams CeresDefaults() { + LevenbergMarquardtParams p; + SetCeresDefaults(&p); + return p; + } + + static LevenbergMarquardtParams EnsureHasOrdering(LevenbergMarquardtParams params, + const NonlinearFactorGraph& graph) { + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); + return params; + } + + static LevenbergMarquardtParams ReplaceOrdering(LevenbergMarquardtParams params, + const Ordering& ordering) { + params.ordering = ordering; + return params; + } + + virtual ~LevenbergMarquardtParams() {} + void print(const std::string& str = "") const override; + + /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @{ + bool getDiagonalDamping() const { return diagonalDamping; } + double getlambdaFactor() const { return lambdaFactor; } + double getlambdaInitial() const { return lambdaInitial; } + double getlambdaLowerBound() const { return lambdaLowerBound; } + double getlambdaUpperBound() const { return lambdaUpperBound; } + std::string getLogFile() const { return logFile; } + std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } + void setlambdaFactor(double value) { lambdaFactor = value; } + void setlambdaInitial(double value) { lambdaInitial = value; } + void setlambdaLowerBound(double value) { lambdaLowerBound = value; } + void setlambdaUpperBound(double value) { lambdaUpperBound = value; } + void setLogFile(const std::string& s) { logFile = s; } + void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} + void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} + // @} +}; + +} diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index f26001b16..4f19f36f8 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -11,15 +11,13 @@ #include #include -#include - namespace gtsam { /* ************************************************************************* */ void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) { if (!linearizationPoint.empty()) { linearizationPoint_ = Values(); - BOOST_FOREACH(const gtsam::Key& key, this->keys()) { + for (Key key : keys()) { linearizationPoint_->insert(key, linearizationPoint.at(key)); } } else { @@ -82,7 +80,7 @@ double LinearContainerFactor::error(const Values& c) const { // Extract subset of values for comparison Values csub; - BOOST_FOREACH(const gtsam::Key& key, keys()) + for (Key key : keys()) csub.insert(key, c.at(key)); // create dummy ordering for evaluation @@ -111,7 +109,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con // Extract subset of values Values subsetC; - BOOST_FOREACH(const gtsam::Key& key, this->keys()) + for (Key key : keys()) subsetC.insert(key, c.at(key)); // Determine delta between linearization points using new ordering @@ -123,10 +121,11 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con jacFactor->getb() = -jacFactor->unweighted_error(delta); } else { HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast(linFactor); - SymmetricBlockMatrix::constBlock Gview = hesFactor->matrixObject().range(0, hesFactor->size(), 0, hesFactor->size()); + + const auto view = hesFactor->informationView(); Vector deltaVector = delta.vector(keys()); - Vector G_delta = Gview.selfadjointView() * deltaVector; - hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm()); + Vector G_delta = view * deltaVector; + hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm().col(0)); hesFactor->linearTerm() -= G_delta; } @@ -166,14 +165,13 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const { } /* ************************************************************************* */ -NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( - const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) -{ +NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph( + const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) { NonlinearFactorGraph result; - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) + result.reserve(linear_graph.size()); + for (const auto& f : linear_graph) if (f) - result.push_back(NonlinearFactorGraph::sharedFactor( - new LinearContainerFactor(f, linearizationPoint))); + result += boost::make_shared(f, linearizationPoint); return result; } diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 307c7f001..928b59e77 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -11,7 +11,6 @@ #include - namespace gtsam { // Forward declarations @@ -141,9 +140,16 @@ public: * Utility function for converting linear graphs to nonlinear graphs * consisting of LinearContainerFactors. */ - static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, + static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph& linear_graph, const Values& linearizationPoint = Values()); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, + const Values& linearizationPoint = Values()) { + return ConvertLinearGraph(linear_graph, linearizationPoint); + } +#endif + protected: void initializeLinearizationPoint(const Values& linearizationPoint); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 40d943656..68da1250e 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -82,9 +82,7 @@ Matrix Marginals::marginalCovariance(Key variable) const { /* ************************************************************************* */ JointMarginal Marginals::jointMarginalCovariance(const std::vector& variables) const { JointMarginal info = jointMarginalInformation(variables); - info.blockMatrix_.full().triangularView() = - info.blockMatrix_.full().selfadjointView().llt().solve( - Matrix::Identity(info.blockMatrix_.full().rows(), info.blockMatrix_.full().rows())).triangularView(); + info.blockMatrix_.invertInPlace(); return info; } @@ -127,7 +125,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab // Get dimensions from factor graph std::vector dims; dims.reserve(variablesSorted.size()); - BOOST_FOREACH(Key key, variablesSorted) { + for(Key key: variablesSorted) { dims.push_back(values_.at(key).dim()); } @@ -144,7 +142,7 @@ VectorValues Marginals::optimize() const { void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "Joint marginal on keys "; bool first = true; - BOOST_FOREACH(Key key, keys_) { + for(Key key: keys_) { if(!first) cout << ", "; else diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index ad723015d..85f694bd2 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -91,13 +91,6 @@ protected: FastMap indices_; public: - /** A block view of the joint marginal - this stores a reference to the - * JointMarginal object, so the JointMarginal object must be kept in scope - * while this block view is needed, otherwise assign this block object to a - * Matrix to store it. - */ - typedef SymmetricBlockMatrix::constBlock Block; - /** Access a block, corresponding to a pair of variables, of the joint * marginal. Each block is accessed by its "vertical position", * corresponding to the variable with nonlinear Key \c iVariable and @@ -111,19 +104,21 @@ public: * @param iVariable The nonlinear Key specifying the "vertical position" of the requested block * @param jVariable The nonlinear Key specifying the "horizontal position" of the requested block */ - Block operator()(Key iVariable, Key jVariable) const { - return blockMatrix_(indices_.at(iVariable), indices_.at(jVariable)); } + Matrix operator()(Key iVariable, Key jVariable) const { + const auto indexI = indices_.at(iVariable); + const auto indexJ = indices_.at(jVariable); + return blockMatrix_.block(indexI, indexJ); + } /** Synonym for operator() */ - Block at(Key iVariable, Key jVariable) const { - return (*this)(iVariable, jVariable); } + Matrix at(Key iVariable, Key jVariable) const { + return (*this)(iVariable, jVariable); + } - /** The full, dense covariance/information matrix of the joint marginal. This returns - * a reference to the JointMarginal object, so the JointMarginal object must be kept - * in scope while this view is needed. Otherwise assign this block object to a Matrix - * to store it. - */ - Eigen::SelfAdjointView fullMatrix() const { return blockMatrix_.matrix(); } + /** The full, dense covariance/information matrix of the joint marginal. */ + Matrix fullMatrix() const { + return blockMatrix_.selfadjointView(); + } /** Print */ void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 6d7196ff5..32aead750 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -17,16 +17,17 @@ */ #include +#include #include #include #include #include -using namespace std; - namespace gtsam { +typedef internal::NonlinearOptimizerState State; + /** * @brief Return the gradient vector of a nonlinear factor graph * @param nfg the graph @@ -40,8 +41,11 @@ static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, return linear->gradientAtZero(); } -double NonlinearConjugateGradientOptimizer::System::error( - const State &state) const { +NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, const Parameters& params) + : Base(graph, std::unique_ptr(new State(initialValues, graph.error(initialValues)))) {} + +double NonlinearConjugateGradientOptimizer::System::error(const State& state) const { return graph_.error(state); } @@ -57,21 +61,26 @@ NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOpt return current.retract(step); } -void NonlinearConjugateGradientOptimizer::iterate() { +GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() { + Values newValues; int dummy; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient( - System(graph_), state_.values, params_, true /* single iterations */); - ++state_.iterations; - state_.error = graph_.error(state_.values); + boost::tie(newValues, dummy) = nonlinearConjugateGradient( + System(graph_), state_->values, params_, true /* single iteration */); + state_.reset(new State(newValues, graph_.error(newValues), state_->iterations + 1)); + + // NOTE(frank): We don't linearize this system, so we must return null here. + return nullptr; } const Values& NonlinearConjugateGradientOptimizer::optimize() { // Optimize until convergence System system(graph_); - boost::tie(state_.values, state_.iterations) = // - nonlinearConjugateGradient(system, state_.values, params_, false); - state_.error = graph_.error(state_.values); - return state_.values; + Values newValues; + int iterations; + boost::tie(newValues, iterations) = + nonlinearConjugateGradient(system, state_->values, params_, false); + state_.reset(new State(std::move(newValues), graph_.error(newValues), iterations)); + return state_->values; } } /* namespace gtsam */ diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 04d4734a4..fa39d2097 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -25,16 +25,7 @@ namespace gtsam { /** An implementation of the nonlinear CG method using the template below */ -class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState { -public: - typedef NonlinearOptimizerState Base; - NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, - const Values& values) : - Base(graph, values) { - } -}; - -class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer { +class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { /* a class for the nonlinearConjugateGradient template */ class System { @@ -63,29 +54,24 @@ public: typedef boost::shared_ptr shared_ptr; protected: - NonlinearConjugateGradientState state_; Parameters params_; + const NonlinearOptimizerParams& _params() const override { + return params_; + } + public: /// Constructor NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, - const Values& initialValues, const Parameters& params = Parameters()) : - Base(graph), state_(graph, initialValues), params_(params) { - } + const Values& initialValues, const Parameters& params = Parameters()); /// Destructor virtual ~NonlinearConjugateGradientOptimizer() { } - virtual void iterate(); - virtual const Values& optimize(); - virtual const NonlinearOptimizerState& _state() const { - return state_; - } - virtual const NonlinearOptimizerParams& _params() const { - return params_; - } + GaussianFactorGraph::shared_ptr iterate() override; + const Values& optimize() override; }; /** Implement the golden-section line search algorithm */ @@ -156,7 +142,7 @@ boost::tuple nonlinearConjugateGradient(const S &system, // GTSAM_CONCEPT_MANIFOLD_TYPE(V); - int iteration = 0; + size_t iteration = 0; // check if we're already close enough double currentError = system.error(initial); diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 0d8d717bc..c6aa52ab2 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -260,7 +260,7 @@ public: std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key()) << ")," << "\n"; this->noiseModel_->print(); - value_.print("Value"); + traits::Print(value_, "Value"); } private: diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 7c8337fc8..5ff022023 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -26,7 +26,7 @@ namespace gtsam { void NonlinearFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s << " keys = { "; - BOOST_FOREACH(Key key, keys()) { + for(Key key: keys()) { std::cout << keyFormatter(key) << " "; } std::cout << "}" << std::endl; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 505f58394..dd4c9123a 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -131,7 +131,8 @@ public: } /** - * Creates a shared_ptr clone of the factor with different keys using + * Creates a shared_ptr clone of the + * factor with different keys using * a map from old->new keys */ shared_ptr rekey(const std::map& rekey_mapping) const; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 77809b51e..56234c13c 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -23,6 +23,8 @@ #include #include #include +#include +#include #include #include #include // for GTSAM_USE_TBB @@ -31,7 +33,6 @@ # include #endif -#include #include #include @@ -133,8 +134,8 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Find bounds double minX = numeric_limits::infinity(), maxX = -numeric_limits::infinity(); double minY = numeric_limits::infinity(), maxY = -numeric_limits::infinity(); - BOOST_FOREACH(Key key, keys) { - if(values.exists(key)) { + for (const Key& key : keys) { + if (values.exists(key)) { boost::optional xy = getXY(values.at(key), formatting); if(xy) { if(xy->x() < minX) @@ -150,7 +151,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } // Create nodes for each variable in the graph - BOOST_FOREACH(Key key, keys){ + for(Key key: keys){ // Label the node with the label from the KeyFormatter stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if(values.exists(key)) { @@ -165,8 +166,8 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, if (formatting.mergeSimilarFactors) { // Remove duplicate factors std::set > structure; - BOOST_FOREACH(const sharedFactor& factor, *this){ - if(factor) { + for (const sharedFactor& factor : factors_) { + if (factor) { vector factorKeys = factor->keys(); std::sort(factorKeys.begin(), factorKeys.end()); structure.insert(factorKeys); @@ -175,7 +176,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections size_t i = 0; - BOOST_FOREACH(const vector& factorKeys, structure){ + for(const vector& factorKeys: structure){ // Make each factor a dot stm << " factor" << i << "[label=\"\", shape=point"; { @@ -187,7 +188,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, stm << "];\n"; // Make factor-variable connections - BOOST_FOREACH(Key key, factorKeys) { + for(Key key: factorKeys) { stm << " var" << key << "--" << "factor" << i << ";\n"; } @@ -195,8 +196,8 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } } else { // Create factors and variable connections - for(size_t i = 0; i < this->size(); ++i) { - const NonlinearFactor::shared_ptr& factor = this->at(i); + for(size_t i = 0; i < size(); ++i) { + const NonlinearFactor::shared_ptr& factor = at(i); if(formatting.plotFactorPoints) { const FastVector& keys = factor->keys(); if (formatting.binaryEdges && keys.size()==2) { @@ -214,7 +215,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Make factor-variable connections if(formatting.connectKeysToFactor && factor) { - BOOST_FOREACH(Key key, *factor) { + for(Key key: *factor) { stm << " var" << key << "--" << "factor" << i << ";\n"; } } @@ -224,7 +225,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, if(factor) { Key k; bool firstTime = true; - BOOST_FOREACH(Key key, *this->at(i)) { + for(Key key: *this->at(i)) { if(firstTime) { k = key; firstTime = false; @@ -246,7 +247,7 @@ double NonlinearFactorGraph::error(const Values& values) const { gttic(NonlinearFactorGraph_error); double total_error = 0.; // iterate over all the factors_ to accumulate the log probabilities - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + for(const sharedFactor& factor: factors_) { if(factor) total_error += factor->error(values); } @@ -270,9 +271,9 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const { // Generate the symbolic factor graph SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared(); - symbolic->reserve(this->size()); + symbolic->reserve(size()); - BOOST_FOREACH(const sharedFactor& factor, *this) { + for (const sharedFactor& factor: factors_) { if(factor) *symbolic += SymbolicFactor(*factor); else @@ -320,17 +321,17 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li #ifdef GTSAM_USE_TBB - linearFG->resize(this->size()); + linearFG->resize(size()); TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP - tbb::parallel_for(tbb::blocked_range(0, this->size()), + tbb::parallel_for(tbb::blocked_range(0, size()), _LinearizeOneFactor(*this, linearizationPoint, *linearFG)); #else - linearFG->reserve(this->size()); + linearFG->reserve(size()); // linearize all factors - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + for(const sharedFactor& factor: factors_) { if(factor) { (*linearFG) += factor->linearize(linearizationPoint); } else @@ -342,10 +343,70 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li return linearFG; } +/* ************************************************************************* */ +static Scatter scatterFromValues(const Values& values, boost::optional ordering) { + gttic(scatterFromValues); + + Scatter scatter; + scatter.reserve(values.size()); + + if (!ordering) { + // use "natural" ordering with keys taken from the initial values + for (const auto& key_value : values) { + scatter.add(key_value.key, key_value.value.dim()); + } + } else { + // copy ordering into keys and lookup dimension in values, is O(n*log n) + for (Key key : *ordering) { + const Value& value = values.at(key); + scatter.add(key, value.dim()); + } + } + + return scatter; +} + +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, boost::optional ordering, const Dampen& dampen) const { + gttic(NonlinearFactorGraph_linearizeToHessianFactor); + + Scatter scatter = scatterFromValues(values, ordering); + + // NOTE(frank): we are heavily leaning on friendship below + HessianFactor::shared_ptr hessianFactor(new HessianFactor(scatter)); + + // Initialize so we can rank-update below + hessianFactor->info_.setZero(); + + // linearize all factors straight into the Hessian + // TODO(frank): this saves on creating the graph, but still mallocs a gaussianFactor! + for (const sharedFactor& nonlinearFactor : factors_) { + if (nonlinearFactor) { + const auto& gaussianFactor = nonlinearFactor->linearize(values); + gaussianFactor->updateHessian(hessianFactor->keys_, &hessianFactor->info_); + } + } + + if (dampen) dampen(hessianFactor); + + return hessianFactor; +} + +/* ************************************************************************* */ +Values NonlinearFactorGraph::updateCholesky(const Values& values, + boost::optional ordering, + const Dampen& dampen) const { + gttic(NonlinearFactorGraph_updateCholesky); + auto hessianFactor = linearizeToHessianFactor(values, ordering, dampen); + VectorValues delta = hessianFactor->solve(); + return values.retract(delta); +} + /* ************************************************************************* */ NonlinearFactorGraph NonlinearFactorGraph::clone() const { NonlinearFactorGraph result; - BOOST_FOREACH(const sharedFactor& f, *this) { + for (const sharedFactor& f : factors_) { if (f) result.push_back(f->clone()); else @@ -357,7 +418,7 @@ NonlinearFactorGraph NonlinearFactorGraph::clone() const { /* ************************************************************************* */ NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map& rekey_mapping) const { NonlinearFactorGraph result; - BOOST_FOREACH(const sharedFactor& f, *this) { + for (const sharedFactor& f : factors_) { if (f) result.push_back(f->rekey(rekey_mapping)); else diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 3702d8a8f..323461459 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -25,6 +25,9 @@ #include #include +#include +#include + namespace gtsam { // Forward declarations @@ -136,14 +139,30 @@ namespace gtsam { */ Ordering orderingCOLAMDConstrained(const FastMap& constraints) const; - /** - * linearize a nonlinear factor graph - */ + /// Linearize a nonlinear factor graph boost::shared_ptr linearize(const Values& linearizationPoint) const; + /// typdef for dampen functions used below + typedef std::function& hessianFactor)> Dampen; + /** - * Clone() performs a deep-copy of the graph, including all of the factors + * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly + * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing + * a new graph, and hence useful in case a dense solve is appropriate for your problem. + * An optional ordering can be given that still decides how the Hessian is laid out. + * An optional lambda function can be used to apply damping on the filled Hessian. + * No parallelism is exploited, because all the factors write in the same memory. */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, boost::optional ordering = boost::none, + const Dampen& dampen = nullptr) const; + + /// Linearize and solve in one pass. + /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values. + Values updateCholesky(const Values& values, boost::optional ordering = boost::none, + const Dampen& dampen = nullptr) const; + + /// Clone() performs a deep-copy of the graph, including all of the factors NonlinearFactorGraph clone() const; /** diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 2405ca222..512069a8a 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -16,12 +16,9 @@ */ #include - #include #include -#include - #include using namespace std; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index bb7b9717b..e1efa2061 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -17,7 +17,7 @@ */ #include - +#include #include #include #include @@ -39,48 +39,74 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -void NonlinearOptimizer::defaultOptimize() { +// NOTE(frank): unique_ptr by-value takes ownership, as discussed in +// http://stackoverflow.com/questions/8114276/ +NonlinearOptimizer::NonlinearOptimizer(const NonlinearFactorGraph& graph, + std::unique_ptr state) + : graph_(graph), state_(std::move(state)) {} - const NonlinearOptimizerParams& params = this->_params(); - double currentError = this->error(); +/* ************************************************************************* */ +NonlinearOptimizer::~NonlinearOptimizer() {} + +/* ************************************************************************* */ +double NonlinearOptimizer::error() const { + return state_->error; +} + +size_t NonlinearOptimizer::iterations() const { + return state_->iterations; +} + +const Values& NonlinearOptimizer::values() const { + return state_->values; +} + +/* ************************************************************************* */ +void NonlinearOptimizer::defaultOptimize() { + const NonlinearOptimizerParams& params = _params(); + double currentError = error(); // check if we're already close enough - if(currentError <= params.errorTol) { + if (currentError <= params.errorTol) { if (params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "Exiting, as error = " << currentError << " < " << params.errorTol << endl; return; } // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("Initial values"); - if (params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "Initial error: " << currentError << endl; + if (params.verbosity >= NonlinearOptimizerParams::VALUES) + values().print("Initial values"); + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + cout << "Initial error: " << currentError << endl; // Return if we already have too many iterations - if(this->iterations() >= params.maxIterations){ + if (iterations() >= params.maxIterations) { if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { - cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl; - } + cout << "iterations: " << iterations() << " >? " << params.maxIterations << endl; + } return; } // Iterative loop do { // Do next iteration - currentError = this->error(); - this->iterate(); + currentError = error(); + iterate(); tictoc_finishedIteration(); // Maybe show output - if(params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("newValues"); - if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << this->error() << endl; - } while(this->iterations() < params.maxIterations && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, - params.errorTol, currentError, this->error(), params.verbosity)); + if (params.verbosity >= NonlinearOptimizerParams::VALUES) + values().print("newValues"); + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + cout << "newError: " << error() << endl; + } while (iterations() < params.maxIterations && + !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, + currentError, error(), params.verbosity)); // Printing if verbose if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { - cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl; - if (this->iterations() >= params.maxIterations) + cout << "iterations: " << iterations() << " >? " << params.maxIterations << endl; + if (iterations() >= params.maxIterations) cout << "Terminating because reached maximum iterations" << endl; } } @@ -98,38 +124,41 @@ const Values& NonlinearOptimizer::optimizeSafely() { } /* ************************************************************************* */ -VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, - const Values& initial, const NonlinearOptimizerParams& params) const { - +VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, + const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point VectorValues delta; + boost::optional optionalOrdering; + if (params.ordering) + optionalOrdering.reset(*params.ordering); // Check which solver we are using if (params.isMultifrontal()) { // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); + delta = gfg.optimize(optionalOrdering, params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(*params.ordering, - params.getEliminationFunction(), boost::none, params.orderingType)->optimize(); + delta = gfg.eliminateSequential(optionalOrdering, params.getEliminationFunction(), boost::none, + params.orderingType)->optimize(); } else if (params.isIterative()) { - // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ..."); - if (boost::shared_ptr pcg = boost::dynamic_pointer_cast(params.iterativeParams) ) { + if (boost::shared_ptr pcg = + boost::dynamic_pointer_cast(params.iterativeParams)) { delta = PCGSolver(*pcg).optimize(gfg); - } - else if (boost::shared_ptr spcg = boost::dynamic_pointer_cast(params.iterativeParams) ) { + } else if (boost::shared_ptr spcg = + boost::dynamic_pointer_cast(params.iterativeParams)) { + if (!params.ordering) + throw std::runtime_error("SubgraphSolver needs an ordering"); delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); - } - else { - throw std::runtime_error("NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ..."); + } else { + throw std::runtime_error( + "NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ..."); } } else { - throw std::runtime_error( - "NonlinearOptimizer::solve: Optimization parameter is invalid"); + throw std::runtime_error("NonlinearOptimizer::solve: Optimization parameter is invalid"); } // return update @@ -138,47 +167,52 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, /* ************************************************************************* */ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, - double errorThreshold, double currentError, double newError, - NonlinearOptimizerParams::Verbosity verbosity) { - - if ( verbosity >= NonlinearOptimizerParams::ERROR ) { - if ( newError <= errorThreshold ) + double errorThreshold, double currentError, double newError, + NonlinearOptimizerParams::Verbosity verbosity) { + if (verbosity >= NonlinearOptimizerParams::ERROR) { + if (newError <= errorThreshold) cout << "errorThreshold: " << newError << " < " << errorThreshold << endl; else cout << "errorThreshold: " << newError << " > " << errorThreshold << endl; } - if ( newError <= errorThreshold ) return true ; + if (newError <= errorThreshold) + return true; // check if diverges double absoluteDecrease = currentError - newError; if (verbosity >= NonlinearOptimizerParams::ERROR) { if (absoluteDecrease <= absoluteErrorTreshold) - cout << "absoluteDecrease: " << setprecision(12) << absoluteDecrease << " < " << absoluteErrorTreshold << endl; + cout << "absoluteDecrease: " << setprecision(12) << absoluteDecrease << " < " + << absoluteErrorTreshold << endl; else - cout << "absoluteDecrease: " << setprecision(12) << absoluteDecrease << " >= " << absoluteErrorTreshold << endl; + cout << "absoluteDecrease: " << setprecision(12) << absoluteDecrease + << " >= " << absoluteErrorTreshold << endl; } // calculate relative error decrease and update currentError double relativeDecrease = absoluteDecrease / currentError; if (verbosity >= NonlinearOptimizerParams::ERROR) { if (relativeDecrease <= relativeErrorTreshold) - cout << "relativeDecrease: " << setprecision(12) << relativeDecrease << " < " << relativeErrorTreshold << endl; + cout << "relativeDecrease: " << setprecision(12) << relativeDecrease << " < " + << relativeErrorTreshold << endl; else - cout << "relativeDecrease: " << setprecision(12) << relativeDecrease << " >= " << relativeErrorTreshold << endl; + cout << "relativeDecrease: " << setprecision(12) << relativeDecrease + << " >= " << relativeErrorTreshold << endl; } - bool converged = (relativeErrorTreshold && (relativeDecrease <= relativeErrorTreshold)) - || (absoluteDecrease <= absoluteErrorTreshold); + bool converged = (relativeErrorTreshold && (relativeDecrease <= relativeErrorTreshold)) || + (absoluteDecrease <= absoluteErrorTreshold); if (verbosity >= NonlinearOptimizerParams::TERMINATION && converged) { - - if(absoluteDecrease >= 0.0) + if (absoluteDecrease >= 0.0) cout << "converged" << endl; else cout << "Warning: stopping nonlinear iterations because error increased" << endl; cout << "errorThreshold: " << newError << " optimize(); +Values result = optimizer->optimize(); // The new optimizer has results and statistics -cout << "Converged in " << final->iterations() << " iterations " - "with final error " << final->error() << endl; - -// The values are a const_shared_ptr (boost::shared_ptr) -Values::const_shared_ptr result = final->values(); - -// Use the results -useTheResult(result); +cout << "Converged in " << optimizer.iterations() << " iterations " + "with final error " << optimizer.error() << endl; \endcode * * Example of setting parameters before optimization: @@ -112,29 +61,23 @@ params.relativeErrorTol = 1e-3; params.absoluteErrorTol = 1e-3; // Optimize -Values::const_shared_ptr result = DoglegOptimizer(graph, initialValues, params).optimized(); +Values result = DoglegOptimizer(graph, initialValues, params).optimize(); \endcode * * This interface also exposes an iterate() method, which performs one - * iteration, returning a NonlinearOptimizer containing the adjusted variable - * assignment. The optimize() method simply calls iterate() multiple times, + * iteration. The optimize() method simply calls iterate() multiple times, * until the error changes less than a threshold. We expose iterate() so that * you can easily control what happens between iterations, such as drawing or * printing, moving points from behind the camera to in front, etc. * - * To modify the graph, values, or parameters between iterations, call the - * update() functions, which preserve all other state (for example, the trust - * region size in DoglegOptimizer). Derived optimizer classes also have - * additional update methods, not in this abstract interface, for updating - * algorithm-specific state. - * - * For more flexibility, since all functions are virtual, you may override them - * in your own derived class. + * For more flexibility you may override virtual methods in your own derived class. */ class GTSAM_EXPORT NonlinearOptimizer { protected: - NonlinearFactorGraph graph_; + NonlinearFactorGraph graph_; ///< The graph with nonlinear factors + + std::unique_ptr state_; ///< PIMPL'd state public: /** A shared pointer to this class */ @@ -163,13 +106,13 @@ public: const Values& optimizeSafely(); /// return error - double error() const { return _state().error; } + double error() const; /// return number of iterations - int iterations() const { return _state().iterations; } + size_t iterations() const; /// return values - const Values& values() const { return _state().values; } + const Values& values() const; /// @} @@ -177,17 +120,17 @@ public: /// @{ /** Virtual destructor */ - virtual ~NonlinearOptimizer() {} + virtual ~NonlinearOptimizer(); /** Default function to do linear solve, i.e. optimize a GaussianFactorGraph */ virtual VectorValues solve(const GaussianFactorGraph &gfg, - const Values& initial, const NonlinearOptimizerParams& params) const; + const NonlinearOptimizerParams& params) const; /** Perform a single iteration, returning a new NonlinearOptimizer class * containing the updated variable assignments, which may be retrieved with * values(). */ - virtual void iterate() = 0; + virtual GaussianFactorGraph::shared_ptr iterate() = 0; /// @} @@ -197,13 +140,11 @@ protected: */ void defaultOptimize(); - virtual const NonlinearOptimizerState& _state() const = 0; - virtual const NonlinearOptimizerParams& _params() const = 0; - /** Constructor for initial construction of base classes. */ - NonlinearOptimizer(const NonlinearFactorGraph& graph) : graph_(graph) {} - + /** Constructor for initial construction of base classes. Takes ownership of state. */ + NonlinearOptimizer(const NonlinearFactorGraph& graph, + std::unique_ptr state); }; /** Check whether the relative error decrease is less than relativeErrorTreshold, diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index ca75bb02a..88fc0f850 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -38,7 +38,7 @@ public: SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR }; - int maxIterations; ///< The maximum iterations to stop iterating (default 100) + size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) @@ -54,7 +54,7 @@ public: } virtual void print(const std::string& str = "") const; - int getMaxIterations() const { + size_t getMaxIterations() const { return maxIterations; } double getRelativeErrorTol() const { diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 446a67ec7..40979b86c 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -26,8 +26,6 @@ #include -#include - #include #include // Only so Eclipse finds class definition @@ -220,7 +218,7 @@ namespace gtsam { /** Constructor from a Filtered view copies out all values */ template Values::Values(const Values::Filtered& view) { - BOOST_FOREACH(const typename Filtered::KeyValuePair& key_value, view) { + for(const typename Filtered::KeyValuePair& key_value: view) { Key key = key_value.key; insert(key, static_cast(key_value.value)); } @@ -229,7 +227,7 @@ namespace gtsam { /* ************************************************************************* */ template Values::Values(const Values::ConstFiltered& view) { - BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { + for(const typename ConstFiltered::KeyValuePair& key_value: view) { Key key = key_value.key; insert(key, static_cast(key_value.value)); } @@ -271,99 +269,74 @@ namespace gtsam { /* ************************************************************************* */ - namespace internal { + namespace internal { - // Check the type and throw exception if incorrect - // Generic version, partially specialized below for various Eigen Matrix types - template - struct handle { - ValueType operator()(Key j, const gtsam::Value * const pointer) { - try { - // value returns a const ValueType&, and the return makes a copy !!!!! - return dynamic_cast&>(*pointer).value(); - } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); - } - } - }; + // Check the type and throw exception if incorrect + // Generic version, partially specialized below for various Eigen Matrix types + template + struct handle { + ValueType operator()(Key j, const Value* const pointer) { + try { + // value returns a const ValueType&, and the return makes a copy !!!!! + return dynamic_cast&>(*pointer).value(); + } catch (std::bad_cast&) { + throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); + } + } + }; - // Handle dynamic vectors - template<> - struct handle > { - Eigen::Matrix operator()(Key j, - const gtsam::Value * const pointer) { - try { - // value returns a const Vector&, and the return makes a copy !!!!! - return dynamic_cast >&>(*pointer).value(); - } catch (std::bad_cast &) { - // If a fixed vector was stored, we end up here as well. - throw ValuesIncorrectType(j, typeid(*pointer), - typeid(Eigen::Matrix)); - } - } - }; + template + struct handle_matrix; - // Handle dynamic matrices - template - struct handle > { - Eigen::Matrix operator()(Key j, - const gtsam::Value * const pointer) { - try { - // value returns a const Matrix&, and the return makes a copy !!!!! - return dynamic_cast >&>(*pointer).value(); - } catch (std::bad_cast &) { - // If a fixed matrix was stored, we end up here as well. - throw ValuesIncorrectType(j, typeid(*pointer), - typeid(Eigen::Matrix)); - } - } - }; + // Handle dynamic matrices + template + struct handle_matrix, true> { + Eigen::Matrix operator()(Key j, const Value* const pointer) { + try { + // value returns a const Matrix&, and the return makes a copy !!!!! + return dynamic_cast>&>(*pointer).value(); + } catch (std::bad_cast&) { + // If a fixed matrix was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); + } + } + }; - // Request for a fixed vector - // TODO(jing): is this piece of code really needed ??? - template - struct handle > { - Eigen::Matrix operator()(Key j, - const gtsam::Value * const pointer) { - try { - // value returns a const VectorM&, and the return makes a copy !!!!! - return dynamic_cast >&>(*pointer).value(); - } catch (std::bad_cast &) { - // Check if a dynamic vector was stored - Matrix A = handle()(j, pointer); // will throw if not.... - // Yes: check size, and throw if not a match - if (A.rows() != M || A.cols() != 1) - throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols()); - else - // This is not a copy because of RVO - return A; - } - } - }; + // Handle fixed matrices + template + struct handle_matrix, false> { + Eigen::Matrix operator()(Key j, const Value* const pointer) { + try { + // value returns a const MatrixMN&, and the return makes a copy !!!!! + return dynamic_cast>&>(*pointer).value(); + } catch (std::bad_cast&) { + Matrix A; + try { + // Check if a dynamic matrix was stored + A = handle_matrix()(j, pointer); // will throw if not.... + } catch (const ValuesIncorrectType&) { + // Or a dynamic vector + A = handle_matrix()(j, pointer); // will throw if not.... + } + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + return A; // copy but not malloc + } + } + }; - // Request for a fixed matrix - template - struct handle > { - Eigen::Matrix operator()(Key j, - const gtsam::Value * const pointer) { - try { - // value returns a const MatrixMN&, and the return makes a copy !!!!! - return dynamic_cast >&>(*pointer).value(); - } catch (std::bad_cast &) { - // Check if a dynamic matrix was stored - Matrix A = handle()(j, pointer); // will throw if not.... - // Yes: check size, and throw if not a match - if (A.rows() != M || A.cols() != N) - throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); - else - // This is not a copy because of RVO - return A; - } - } - }; + // Handle matrices + template + struct handle> { + Eigen::Matrix operator()(Key j, const Value* const pointer) { + return handle_matrix, + (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer); + } + }; - - } // internal + } // internal /* ************************************************************************* */ template @@ -402,47 +375,16 @@ namespace gtsam { /* ************************************************************************* */ - // wrap all fixed in dynamics when insert and update - namespace internal { - - // general type - template - struct handle_wrap { - inline gtsam::GenericValue operator()(Key j, const ValueType& val) { - return gtsam::GenericValue(val); - } - }; - - // when insert/update a fixed size vector: convert to dynamic size - template - struct handle_wrap > { - inline gtsam::GenericValue > operator()( - Key j, const Eigen::Matrix& val) { - return gtsam::GenericValue >(val); - } - }; - - // when insert/update a fixed size matrix: convert to dynamic size - template - struct handle_wrap > { - inline gtsam::GenericValue > operator()( - Key j, const Eigen::Matrix& val) { - return gtsam::GenericValue >(val); - } - }; - - } - // insert a templated value template void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(internal::handle_wrap()(j, val))); + insert(j, static_cast(GenericValue(val))); } // update with templated value template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(internal::handle_wrap()(j, val))); + update(j, static_cast(GenericValue(val))); } } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index ba7a040cd..1bd8b3a73 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,7 +25,6 @@ #include #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -37,6 +36,7 @@ #include #include +#include #include using namespace std; @@ -48,12 +48,32 @@ namespace gtsam { this->insert(other); } + /* ************************************************************************* */ + Values::Values(Values&& other) : values_(std::move(other.values_)) { + } + + /* ************************************************************************* */ + Values::Values(const Values& other, const VectorValues& delta) { + for (const_iterator key_value = other.begin(); key_value != other.end(); ++key_value) { + VectorValues::const_iterator it = delta.find(key_value->key); + Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument + if (it != delta.end()) { + const Vector& v = it->second; + Value* retractedValue(key_value->value.retract_(v)); // Retract + values_.insert(key, retractedValue); // Add retracted result directly to result values + } else { + values_.insert(key, key_value->value.clone_()); // Add original version to result values + } + } + } + /* ************************************************************************* */ void Values::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str << "Values with " << size() << " values:" << endl; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { cout << "Value " << keyFormatter(key_value->key) << ": "; key_value->value.print(""); + cout << "\n"; } } @@ -79,23 +99,8 @@ namespace gtsam { } /* ************************************************************************* */ - Values Values::retract(const VectorValues& delta) const - { - Values result; - - for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - VectorValues::const_iterator vector_item = delta.find(key_value->key); - Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument - if(vector_item != delta.end()) { - const Vector& singleDelta = vector_item->second; - Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract - result.values_.insert(key, retractedValue); // Add retracted result directly to result values - } else { - result.values_.insert(key, key_value->value.clone_()); // Add original version to result values - } - } - - return result; + Values Values::retract(const VectorValues& delta) const { + return Values(*this, delta); } /* ************************************************************************* */ @@ -194,7 +199,7 @@ namespace gtsam { /* ************************************************************************* */ size_t Values::dim() const { size_t result = 0; - BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { + for(const ConstKeyValuePair& key_value: *this) { result += key_value.value.dim(); } return result; @@ -203,7 +208,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues Values::zeroVectors() const { VectorValues result; - BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) + for(const ConstKeyValuePair& key_value: *this) result.insert(key_value.key, Vector::Zero(key_value.value.dim())); return result; } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index a61d01f23..bf17f1f0d 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -146,6 +146,12 @@ namespace gtsam { /** Copy constructor duplicates all keys and values */ Values(const Values& other); + /** Move constructor */ + Values(Values&& other); + + /** Construct from a Values and an update vector: identical to other.retract(delta) */ + Values(const Values& other, const VectorValues& delta); + /** Constructor from a Filtered view copies out all values */ template Values(const Filtered& view); diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index a256f8fe2..c0eccbd08 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -22,8 +22,6 @@ #include #include -#include - namespace gtsam { /** @@ -49,7 +47,7 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, // Loop over all variables const double one_over_2delta = 1.0 / (2.0 * delta); VectorValues dX = values.zeroVectors(); - BOOST_FOREACH(Key key, factor) { + for(Key key: factor) { // Compute central differences using the values struct. const size_t cols = dX.dim(key); Matrix J = Matrix::Zero(rows, cols); diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h index 6616f0a83..e58c40203 100644 --- a/gtsam/nonlinear/internal/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ed811764a..a147f505e 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -25,6 +25,7 @@ #include #include +#include namespace gtsam { namespace internal { diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 966f43da8..d11908b54 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -175,7 +175,7 @@ public: /// Print virtual void print(const std::string& indent = "") const { - std::cout << indent << "Leaf, key = " << key_ << std::endl; + std::cout << indent << "Leaf, key = " << DefaultKeyFormatter(key_) << std::endl; } /// Return keys that play in this expression @@ -310,11 +310,13 @@ public: assert(reinterpret_cast(ptr) % TraceAlignment == 0); // Create a Record in the memory pointed to by ptr - // Calling the construct will record the traces for all arguments + // Calling the constructor will record the traces for all arguments // Write an Expression execution trace in record->trace // Iff Constant or Leaf, this will not write to traceStorage, only to trace. // Iff the expression is functional, write all Records in traceStorage buffer // Return value of type T is recorded in record->value + // NOTE(frank, abe): The destructor on this record is never called due to this placement new + // Records must only contain statically sized objects! Record* record = new (ptr) Record(values, *expression1_, ptr); // Our trace parameter is set to point to the Record @@ -389,6 +391,7 @@ public: ExecutionTrace trace1; ExecutionTrace trace2; + // TODO(frank): These aren't needed kill them! A1 value1; A2 value2; @@ -632,91 +635,82 @@ class ScalarMultiplyNode : public ExpressionNode { } }; + //----------------------------------------------------------------------------- -/// Sum Expression +/// Binary Sum Expression template -class SumExpressionNode : public ExpressionNode { +class BinarySumNode : public ExpressionNode { typedef ExpressionNode NodeT; - std::vector> expressions_; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; public: - explicit SumExpressionNode(const std::vector>& expressions) { + explicit BinarySumNode() { this->traceSize_ = upAligned(sizeof(Record)); - for (const Expression& e : expressions) - add(e); } - void add(const Expression& e) { - expressions_.push_back(e.root()); - this->traceSize_ += e.traceSize(); + /// Constructor with a binary function f, and two input arguments + BinarySumNode(const Expression& e1, const Expression& e2) + : expression1_(e1.root()), expression2_(e2.root()) { + this->traceSize_ = // + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } /// Destructor - virtual ~SumExpressionNode() {} - - size_t nrTerms() const { - return expressions_.size(); - } + virtual ~BinarySumNode() {} /// Print virtual void print(const std::string& indent = "") const { - std::cout << indent << "SumExpressionNode" << std::endl; - for (const auto& node : expressions_) - node->print(indent + " "); + std::cout << indent << "BinarySumNode" << std::endl; + expression1_->print(indent + " "); + expression2_->print(indent + " "); } /// Return value virtual T value(const Values& values) const { - auto it = expressions_.begin(); - T sum = (*it)->value(values); - for (++it; it != expressions_.end(); ++it) - sum = sum + (*it)->value(values); - return sum; + return expression1_->value(values) + expression2_->value(values); } /// Return keys that play in this expression virtual std::set keys() const { - std::set keys; - for (const auto& node : expressions_) { - std::set myKeys = node->keys(); - keys.insert(myKeys.begin(), myKeys.end()); - } + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); return keys; } /// Return dimensions for each argument virtual void dims(std::map& map) const { - for (const auto& node : expressions_) - node->dims(map); + expression1_->dims(map); + expression2_->dims(map); } // Inner Record Class struct Record : public CallRecordImplementor::dimension> { - std::vector> traces_; - - explicit Record(size_t nrArguments) : traces_(nrArguments) {} + ExecutionTrace trace1; + ExecutionTrace trace2; /// Print to std::cout void print(const std::string& indent) const { - std::cout << indent << "SumExpressionNode::Record {" << std::endl; - for (const auto& trace : traces_) - trace.print(indent); + std::cout << indent << "BinarySumNode::Record {" << std::endl; + trace1.print(indent); + trace2.print(indent); std::cout << indent << "}" << std::endl; } - /// If the SumExpression is the root, we just start as many pipelines as there are terms. + /// If the BinarySumExpression is the root, we just start as many pipelines as there are terms. void startReverseAD4(JacobianMap& jacobians) const { - for (const auto& trace : traces_) - // NOTE(frank): equivalent to trace.reverseAD1(dTdA, jacobians) with dTdA=Identity - trace.startReverseAD1(jacobians); + // NOTE(frank): equivalent to trace.reverseAD1(dTdA, jacobians) with dTdA=Identity + trace1.startReverseAD1(jacobians); + trace2.startReverseAD1(jacobians); } /// If we are not the root, we simply pass on the adjoint matrix dFdT to all terms template void reverseAD4(const MatrixType& dFdT, JacobianMap& jacobians) const { - for (const auto& trace : traces_) - // NOTE(frank): equivalent to trace.reverseAD1(dFdT * dTdA, jacobians) with dTdA=Identity - trace.reverseAD1(dFdT, jacobians); + // NOTE(frank): equivalent to trace.reverseAD1(dFdT * dTdA, jacobians) with dTdA=Identity + trace1.reverseAD1(dFdT, jacobians); + trace2.reverseAD1(dFdT, jacobians); } }; @@ -724,17 +718,13 @@ class SumExpressionNode : public ExpressionNode { virtual T traceExecution(const Values& values, ExecutionTrace& trace, ExecutionTraceStorage* ptr) const { assert(reinterpret_cast(ptr) % TraceAlignment == 0); - size_t nrArguments = expressions_.size(); - Record* record = new (ptr) Record(nrArguments); - ptr += upAligned(sizeof(Record)); - size_t i = 0; - T sum = traits::Identity(); - for (const auto& node : expressions_) { - sum = sum + node->traceExecution(values, record->traces_[i++], ptr); - ptr += node->traceSize(); - } + Record* record = new (ptr) Record(); trace.setFunction(record); - return sum; + + ExecutionTraceStorage* ptr1 = ptr + upAligned(sizeof(Record)); + ExecutionTraceStorage* ptr2 = ptr1 + expression1_->traceSize(); + return expression1_->traceExecution(values, record->trace1, ptr1) + + expression2_->traceExecution(values, record->trace2, ptr2); } }; diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h new file mode 100644 index 000000000..19a390c45 --- /dev/null +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + + * 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 LevenbergMarquardtState.h + * @brief A LevenbergMarquardtState class containing most of the logic for Levenberg-Marquardt + * @author Frank Dellaert + * @date April 2016 + */ + +#include "NonlinearOptimizerState.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace gtsam { + +namespace internal { + +// TODO(frank): once Values supports move, we can make State completely functional. +// As it is now, increaseLambda is non-const or otherwise we make a Values copy every time +// decreaseLambda would also benefit from a working Values move constructor +struct LevenbergMarquardtState : public NonlinearOptimizerState { + typedef LevenbergMarquardtState This; + + // TODO(frank): make these const once increaseLambda can be made const + double lambda; + double currentFactor; + int totalNumberInnerIterations; ///< The total number of inner iterations in the + // optimization (for each iteration, LM tries multiple + // inner iterations with different lambdas) + + LevenbergMarquardtState(const Values& initialValues, double error, double lambda, + double currentFactor, unsigned int iterations = 0, + unsigned int totalNumberInnerIterations = 0) + : NonlinearOptimizerState(initialValues, error, iterations), + lambda(lambda), + currentFactor(currentFactor), + totalNumberInnerIterations(totalNumberInnerIterations) {} + + // Constructor version that takes ownership of values + LevenbergMarquardtState(Values&& initialValues, double error, double lambda, double currentFactor, + unsigned int iterations = 0, unsigned int totalNumberInnerIterations = 0) + : NonlinearOptimizerState(initialValues, error, iterations), + lambda(lambda), + currentFactor(currentFactor), + totalNumberInnerIterations(totalNumberInnerIterations) {} + + // Applies policy to *increase* lambda: should be used if the current update was NOT successful + // TODO(frank): non-const method until Values properly support std::move + void increaseLambda(const LevenbergMarquardtParams& params) { + lambda *= currentFactor; + totalNumberInnerIterations += 1; + if (!params.useFixedLambdaFactor) { + currentFactor *= 2.0; + } + } + + // Apply policy to decrease lambda if the current update was successful + // stepQuality not used in the naive policy) + // Take ownsership of newValues, must be passed an rvalue + std::unique_ptr decreaseLambda(const LevenbergMarquardtParams& params, double stepQuality, + Values&& newValues, double newError) const { + double newLambda = lambda, newFactor = currentFactor; + if (params.useFixedLambdaFactor) { + newLambda /= currentFactor; + } else { + // TODO(frank): odd that currentFactor is not used to change lambda here... + newLambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); + newFactor = 2.0 * currentFactor; + } + newLambda = std::max(params.lambdaLowerBound, newLambda); + return std::unique_ptr(new This(std::move(newValues), newError, newLambda, newFactor, + iterations + 1, totalNumberInnerIterations + 1)); + } + + /** Small struct to cache objects needed for damping. + * This is used in buildDampedSystem */ + struct CachedModel { + CachedModel() {} // default int makes zero-size matrices + CachedModel(int dim, double sigma) + : A(Matrix::Identity(dim, dim)), + b(Vector::Zero(dim)), + model(noiseModel::Isotropic::Sigma(dim, sigma)) {} + CachedModel(int dim, double sigma, const Vector& diagonal) + : A(Eigen::DiagonalMatrix(diagonal)), + b(Vector::Zero(dim)), + model(noiseModel::Isotropic::Sigma(dim, sigma)) {} + Matrix A; + Vector b; + SharedDiagonal model; + }; + + // Small cache of A|b|model indexed by dimension. Can save many mallocs. + mutable std::vector noiseModelCache; + CachedModel* getCachedModel(size_t dim) const { + if (dim > noiseModelCache.size()) + noiseModelCache.resize(dim); + CachedModel* item = &noiseModelCache[dim - 1]; + if (!item->model) + *item = CachedModel(dim, 1.0 / std::sqrt(lambda)); + return item; + }; + + /// Build a damped system for a specific lambda, vanilla version + GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped /* gets copied */) const { + noiseModelCache.resize(0); + // for each of the variables, add a prior + damped.reserve(damped.size() + values.size()); + for (const auto& key_value : values) { + const Key key = key_value.key; + const size_t dim = key_value.value.dim(); + const CachedModel* item = getCachedModel(dim); + damped += boost::make_shared(key, item->A, item->b, item->model); + } + return damped; + } + + /// Build a damped system, use hessianDiagonal per variable (more expensive) + GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped, // gets copied + const VectorValues& sqrtHessianDiagonal) const { + noiseModelCache.resize(0); + damped.reserve(damped.size() + values.size()); + for (const auto& key_vector : sqrtHessianDiagonal) { + try { + const Key key = key_vector.first; + const size_t dim = key_vector.second.size(); + CachedModel* item = getCachedModel(dim); + item->A.diagonal() = sqrtHessianDiagonal.at(key); // use diag(hessian) + damped += boost::make_shared(key, item->A, item->b, item->model); + } catch (const std::out_of_range& e) { + continue; // Don't attempt any damping if no key found in diagonal + } + } + return damped; + } +}; + +} // namespace internal + +} /* namespace gtsam */ diff --git a/gtsam/nonlinear/internal/NonlinearOptimizerState.h b/gtsam/nonlinear/internal/NonlinearOptimizerState.h new file mode 100644 index 000000000..0c65d91f9 --- /dev/null +++ b/gtsam/nonlinear/internal/NonlinearOptimizerState.h @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * 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 NonlinearOptimizerState.h + * @brief Private class for NonlinearOptimizer state + * @author Richard Roberts + * @author Frank Dellaert + * @date Sep 7, 2009 + */ + +#pragma once + +#include + +namespace gtsam { +namespace internal { + +/** + * Base class for a nonlinear optimization state, including the current estimate + * of the variable values, error, and number of iterations. Optimizers derived + * from NonlinearOptimizer usually also define a derived state class containing + * additional state specific to the algorithm (for example, Dogleg state + * contains the current trust region radius). + */ +struct NonlinearOptimizerState { + public: + /** The current estimate of the variable values. */ + const Values values; + + /** The factor graph error on the current values. */ + const double error; + + /** The number of optimization iterations performed. */ + const size_t iterations; + + virtual ~NonlinearOptimizerState() {} + + NonlinearOptimizerState(const Values& values, double error, size_t iterations = 0) + : values(values), error(error), iterations(iterations) {} + + // Constructor version that takes ownership of values + NonlinearOptimizerState(Values&& values, double error, size_t iterations = 0) + : values(std::move(values)), error(error), iterations(iterations) {} +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 208f0b284..5fc4e208d 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -23,6 +23,7 @@ #include #include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index dece9bad8..bde79e780 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -80,7 +80,7 @@ TEST(Expression, Leaves) { // Unary(Leaf) namespace unary { Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) { - return Point2(); + return Point2(0,0); } double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; @@ -111,24 +111,43 @@ TEST(Expression, Unary3) { } /* ************************************************************************* */ +class Class : public Point3 { + public: + enum {dimension = 3}; + using Point3::Point3; + const Vector3& vector() const { return *this; } + inline static Class identity() { return Class(0,0,0); } + double norm(OptionalJacobian<1,3> H = boost::none) const { + return norm3(*this, H); + } + bool equals(const Class &q, double tol) const { + return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); + } + void print(const string& s) const { cout << s << *this << endl;} +}; + +namespace gtsam { +template<> struct traits : public internal::VectorSpace {}; +} + // Nullary Method TEST(Expression, NullaryMethod) { // Create expression - Expression p(67); - Expression norm(>sam::norm, p); + Expression p(67); + Expression norm_(p, &Class::norm); // Create Values Values values; - values.insert(67, Point3(3, 4, 5)); + values.insert(67, Class(3, 4, 5)); // Check dims as map std::map map; - norm.dims(map); + norm_.dims(map); LONGS_EQUAL(1, map.size()); // Get value and Jacobians std::vector H(1); - double actual = norm.value(values, H); + double actual = norm_.value(values, H); // Check all EXPECT(actual == sqrt(50)); @@ -288,18 +307,18 @@ TEST(Expression, ternary) { /* ************************************************************************* */ TEST(Expression, ScalarMultiply) { const Key key(67); - const Point3_ sum_ = 23 * Point3_(key); + const Point3_ expr = 23 * Point3_(key); set expected_keys = list_of(key); - EXPECT(expected_keys == sum_.keys()); + EXPECT(expected_keys == expr.keys()); map actual_dims, expected_dims = map_list_of(key, 3); - sum_.dims(actual_dims); + expr.dims(actual_dims); EXPECT(actual_dims == expected_dims); // Check dims as map std::map map; - sum_.dims(map); + expr.dims(map); LONGS_EQUAL(1, map.size()); Values values; @@ -307,16 +326,16 @@ TEST(Expression, ScalarMultiply) { // Check value const Point3 expected(23, 0, 46); - EXPECT(assert_equal(expected, sum_.value(values))); + EXPECT(assert_equal(expected, expr.value(values))); // Check value + Jacobians std::vector H(1); - EXPECT(assert_equal(expected, sum_.value(values, H))); + EXPECT(assert_equal(expected, expr.value(values, H))); EXPECT(assert_equal(23 * I_3x3, H[0])); } /* ************************************************************************* */ -TEST(Expression, Sum) { +TEST(Expression, BinarySum) { const Key key(67); const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1)); @@ -349,9 +368,31 @@ TEST(Expression, Sum) { TEST(Expression, TripleSum) { const Key key(67); const Point3_ p1_(Point3(1, 1, 1)), p2_(key); - const SumExpression sum_ = p1_ + p2_ + p1_; + const Expression sum_ = p1_ + p2_ + p1_; + + LONGS_EQUAL(1, sum_.keys().size()); + + Values values; + values.insert(key, Point3(2, 2, 2)); + + // Check value + const Point3 expected(4, 4, 4); + EXPECT(assert_equal(expected, sum_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, sum_.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, PlusEqual) { + const Key key(67); + const Point3_ p1_(Point3(1, 1, 1)), p2_(key); + Expression sum_ = p1_; + sum_ += p2_; + sum_ += p1_; - LONGS_EQUAL(3, sum_.nrTerms()); LONGS_EQUAL(1, sum_.keys().size()); Values values; @@ -370,7 +411,7 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const Double_ norm_(>sam::norm, Point3_(key)); + const Double_ norm_(>sam::norm3, Point3_(key)); const Double_ sum_ = norm_ + norm_; Values values; @@ -389,7 +430,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const Double_ norm_(>sam::norm, sum_); + const Double_ norm_(>sam::norm3, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 8c1c85038..c82ba3391 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -31,6 +31,7 @@ using namespace boost::assign; #include #include +#include using namespace gtsam; using namespace std; @@ -47,9 +48,9 @@ class TestValueData { public: static int ConstructorCount; static int DestructorCount; - TestValueData(const TestValueData& other) { /*cout << "Copy constructor" << endl;*/ ++ ConstructorCount; } - TestValueData() { /*cout << "Default constructor" << endl;*/ ++ ConstructorCount; } - ~TestValueData() { /*cout << "Destructor" << endl;*/ ++ DestructorCount; } + TestValueData(const TestValueData& other) { ++ ConstructorCount; } + TestValueData() { ++ ConstructorCount; } + ~TestValueData() { ++ DestructorCount; } }; int TestValueData::ConstructorCount = 0; int TestValueData::DestructorCount = 0; @@ -76,7 +77,6 @@ namespace gtsam { template <> struct traits : public internal::Manifold {}; } - /* ************************************************************************* */ TEST( Values, equals1 ) { @@ -198,28 +198,14 @@ TEST(Values, basic_functions) } -///* ************************************************************************* */ -//TEST(Values, dim_zero) -//{ -// Values config0; -// config0.insert(key1, Vector2(Vector2(2.0, 3.0)); -// config0.insert(key2, Vector3(5.0, 6.0, 7.0)); -// LONGS_EQUAL(5, config0.dim()); -// -// VectorValues expected; -// expected.insert(key1, zero(2)); -// expected.insert(key2, zero(3)); -// CHECK(assert_equal(expected, config0.zero())); -//} - /* ************************************************************************* */ -TEST(Values, expmap_a) +TEST(Values, retract_full) { Values config0; config0.insert(key1, Vector3(1.0, 2.0, 3.0)); config0.insert(key2, Vector3(5.0, 6.0, 7.0)); - VectorValues increment = pair_list_of + VectorValues delta = pair_list_of (key1, Vector3(1.0, 1.1, 1.2)) (key2, Vector3(1.3, 1.4, 1.5)); @@ -227,51 +213,35 @@ TEST(Values, expmap_a) expected.insert(key1, Vector3(2.0, 3.1, 4.2)); expected.insert(key2, Vector3(6.3, 7.4, 8.5)); - CHECK(assert_equal(expected, config0.retract(increment))); + CHECK(assert_equal(expected, config0.retract(delta))); + CHECK(assert_equal(expected, Values(config0, delta))); } /* ************************************************************************* */ -TEST(Values, expmap_b) +TEST(Values, retract_partial) { Values config0; config0.insert(key1, Vector3(1.0, 2.0, 3.0)); config0.insert(key2, Vector3(5.0, 6.0, 7.0)); - VectorValues increment = pair_list_of + VectorValues delta = pair_list_of (key2, Vector3(1.3, 1.4, 1.5)); Values expected; expected.insert(key1, Vector3(1.0, 2.0, 3.0)); expected.insert(key2, Vector3(6.3, 7.4, 8.5)); - CHECK(assert_equal(expected, config0.retract(increment))); + CHECK(assert_equal(expected, config0.retract(delta))); + CHECK(assert_equal(expected, Values(config0, delta))); } /* ************************************************************************* */ -//TEST(Values, expmap_c) -//{ -// Values config0; -// config0.insert(key1, Vector3(1.0, 2.0, 3.0)); -// config0.insert(key2, Vector3(5.0, 6.0, 7.0)); -// -// Vector increment = Vector6( -// 1.0, 1.1, 1.2, -// 1.3, 1.4, 1.5); -// -// Values expected; -// expected.insert(key1, Vector3(2.0, 3.1, 4.2)); -// expected.insert(key2, Vector3(6.3, 7.4, 8.5)); -// -// CHECK(assert_equal(expected, config0.retract(increment))); -//} - -/* ************************************************************************* */ -TEST(Values, expmap_d) +TEST(Values, equals) { Values config0; config0.insert(key1, Vector3(1.0, 2.0, 3.0)); config0.insert(key2, Vector3(5.0, 6.0, 7.0)); - //config0.print("config0"); + CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); @@ -279,8 +249,8 @@ TEST(Values, expmap_d) poseconfig.insert(key1, Pose2(1, 2, 3)); poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5)); - CHECK(equal(config0, config0)); - CHECK(config0.equals(config0)); + CHECK(equal(poseconfig, poseconfig)); + CHECK(poseconfig.equals(poseconfig)); } /* ************************************************************************* */ @@ -366,7 +336,7 @@ TEST(Values, filter) { int i = 0; Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); EXPECT_LONGS_EQUAL(2, (long)filtered.size()); - BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { + for(const Values::Filtered<>::KeyValuePair& key_value: filtered) { if(i == 0) { LONGS_EQUAL(2, (long)key_value.key); try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");} @@ -401,7 +371,7 @@ TEST(Values, filter) { i = 0; Values::ConstFiltered pose_filtered = values.filter(); EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size()); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, pose_filtered) { + for(const Values::ConstFiltered::KeyValuePair& key_value: pose_filtered) { if(i == 0) { EXPECT_LONGS_EQUAL(1, (long)key_value.key); EXPECT(assert_equal(pose1, key_value.value)); @@ -437,7 +407,7 @@ TEST(Values, Symbol_filter) { values.insert(Symbol('y', 3), pose3); int i = 0; - BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { + for(const Values::Filtered::KeyValuePair& key_value: values.filter(Symbol::ChrTest('y'))) { if(i == 0) { LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); EXPECT(assert_equal(pose1, key_value.value.cast())); @@ -453,8 +423,8 @@ TEST(Values, Symbol_filter) { } /* ************************************************************************* */ +// Check that Value destructors are called when Values container is deleted TEST(Values, Destructors) { - // Check that Value destructors are called when Values container is deleted { Values values; { @@ -469,11 +439,95 @@ TEST(Values, Destructors) { // GenericValue in insert() // but I'm sure some advanced programmer can figure out // a way to avoid the temporary, or optimize it out - LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount); - LONGS_EQUAL(2+2, (long)TestValueData::DestructorCount); + LONGS_EQUAL(4 + 2, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(2 + 2, (long)TestValueData::DestructorCount); + } + LONGS_EQUAL(4 + 2, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(4 + 2, (long)TestValueData::DestructorCount); +} + +/* ************************************************************************* */ +TEST(Values, copy_constructor) { + { + Values values; + TestValueData::ConstructorCount = 0; + TestValueData::DestructorCount = 0; + { + TestValue value1; + TestValue value2; + EXPECT_LONGS_EQUAL(2, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(0, (long)TestValueData::DestructorCount); + values.insert(0, value1); + values.insert(1, value2); + } + EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(4, (long)TestValueData::DestructorCount); + + // Copy constructor + { + Values copied(values); // makes 2 extra copies + EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(4, (long)TestValueData::DestructorCount); + } + EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(6, (long)TestValueData::DestructorCount); // copied destructed ! + } + EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(8, (long)TestValueData::DestructorCount); // values destructed ! +} + +/* ************************************************************************* */ +// small class with a constructor to create an rvalue +struct TestValues : Values { + using Values::Values; // inherits move constructor + + TestValues(const TestValue& value1, const TestValue& value2) { + insert(0, value1); + insert(1, value2); + } +}; +static_assert(std::is_move_constructible::value, ""); +static_assert(std::is_move_constructible::value, ""); + +// test move semantics +TEST(Values, move_constructor) { + { + TestValueData::ConstructorCount = 0; + TestValueData::DestructorCount = 0; + TestValue value1; + TestValue value2; + EXPECT_LONGS_EQUAL(2, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(0, (long)TestValueData::DestructorCount); + TestValues values(TestValues(value1, value2)); // Move happens here ! (could be optimization?) + EXPECT_LONGS_EQUAL(2, values.size()); + EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount); // yay ! We don't copy + EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); // extra insert copies + } + EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(6, (long)TestValueData::DestructorCount); +} + +// test use of std::move +TEST(Values, std_move) { + { + TestValueData::ConstructorCount = 0; + TestValueData::DestructorCount = 0; + { + TestValue value1; + TestValue value2; + TestValues values(value1, value2); + EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); + EXPECT_LONGS_EQUAL(2, values.size()); + TestValues moved(std::move(values)); // Move happens here ! + EXPECT_LONGS_EQUAL(2, values.size()); // Uh oh! Should be 0 ! + EXPECT_LONGS_EQUAL(2, moved.size()); + EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount); // Uh oh! Should be 6 :-( + EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); // extra insert copies + } + EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(8, (long)TestValueData::DestructorCount); } - LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount); - LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount); } /* ************************************************************************* */ @@ -511,16 +565,18 @@ TEST(Values, VectorFixedInsertFixedRead) { } /* ************************************************************************* */ -TEST(Values, VectorFixedInsertDynamicRead) { - Values values; - Vector3 v; v << 5.0, 6.0, 7.0; - values.insert(key1, v); - Vector expected(3); expected << 5.0, 6.0, 7.0; - Vector actual = values.at(key1); - LONGS_EQUAL(3, actual.rows()); - LONGS_EQUAL(1, actual.cols()); - CHECK(assert_equal(expected, actual)); -} +// NOTE(frank): test is broken, because the scheme it tested was *very* slow. +// TODO(frank): find long-term solution. that works w matlab/python. +//TEST(Values, VectorFixedInsertDynamicRead) { +// Values values; +// Vector3 v; v << 5.0, 6.0, 7.0; +// values.insert(key1, v); +// Vector expected(3); expected << 5.0, 6.0, 7.0; +// Vector actual = values.at(key1); +// LONGS_EQUAL(3, actual.rows()); +// LONGS_EQUAL(1, actual.cols()); +// CHECK(assert_equal(expected, actual)); +//} /* ************************************************************************* */ TEST(Values, MatrixDynamicInsertFixedRead) { @@ -531,6 +587,7 @@ TEST(Values, MatrixDynamicInsertFixedRead) { CHECK(assert_equal((Vector)expected, values.at(key1))); CHECK_EXCEPTION(values.at(key1), exception); } + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index bfd7d4e34..e6aab45da 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -92,7 +92,7 @@ namespace gtsam { T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR - typename traits::ChartJacobian::Fixed Hlocal; + typename traits::ChartJacobian::Jacobian Hlocal; Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); if (H1) *H1 = Hlocal * (*H1); if (H1) *H2 = Hlocal * (*H2); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index e668d27d3..e6b312b95 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -139,7 +139,7 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.transpose() << ")' and (" << pn_.vector().transpose() + << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'" << std::endl; } @@ -162,7 +162,7 @@ public: // The point d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate - Point2 pn; + Point2 pn(0,0); if (!DE && !Dd) { Point3 _1T2 = E.direction().point3(); @@ -195,7 +195,7 @@ public: } Point2 reprojectionError = pn - pn_; - return f_ * reprojectionError.vector(); + return f_ * reprojectionError; } }; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 0f187db75..9356aceb2 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -107,7 +107,7 @@ public: */ void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /** @@ -115,15 +115,14 @@ public: */ bool equals(const NonlinearFactor &p, double tol = 1e-9) const { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ Vector evaluateError(const CAMERA& camera, const LANDMARK& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const { try { - Point2 reprojError(camera.project2(point,H1,H2) - measured_); - return reprojError.vector(); + return camera.project2(point,H1,H2) - measured_; } catch( CheiralityException& e) { if (H1) *H1 = JacobianC::Zero(); @@ -145,8 +144,7 @@ public: try { const CAMERA& camera = values.at(key1); const LANDMARK& point = values.at(key2); - Point2 reprojError(camera.project2(point, H1, H2) - measured()); - b = -reprojError.vector(); + b = measured() - camera.project2(point, H1, H2); } catch (CheiralityException& e) { H1.setZero(); H2.setZero(); @@ -243,7 +241,7 @@ public: */ void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /** @@ -251,7 +249,7 @@ public: */ bool equals(const NonlinearFactor &p, double tol = 1e-9) const { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ @@ -262,8 +260,7 @@ public: { try { Camera camera(pose3,calib); - Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); - return reprojError.vector(); + return camera.project(point, H1, H2, H3) - measured_; } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2, 6); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 180e5cd24..58408e7e3 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -43,7 +43,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); - BOOST_FOREACH(const boost::shared_ptr& factor, g) { + for(const boost::shared_ptr& factor: g) { Matrix3 Rij; boost::shared_ptr > pose3Between = @@ -75,7 +75,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; - BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) { + for(const VectorValues::value_type& it: relaxedRot3) { Key key = it.first; if (key != keyAnchor) { const Vector& rotVector = it.second; @@ -108,7 +108,7 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; - BOOST_FOREACH(const boost::shared_ptr& factor, graph) { + for(const boost::shared_ptr& factor: graph) { // recast to a between on Pose3 boost::shared_ptr > pose3Between = @@ -120,9 +120,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { boost::shared_ptr > pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.add( - BetweenFactor(keyAnchor, pose3Prior->keys()[0], - pose3Prior->prior(), pose3Prior->noiseModel())); + pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } @@ -150,7 +149,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; inverseRot.insert(keyAnchor, Rot3()); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) { + for(const Values::ConstKeyValuePair& key_value: givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); inverseRot.insert(key, pose.rotation().inverse()); @@ -165,7 +164,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; VectorValues grad; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + for(const Values::ConstKeyValuePair& key_value: inverseRot) { Key key = key_value.key; grad.insert(key,Vector3::Zero()); size_t currNodeDeg = (adjEdgesMap.at(key)).size(); @@ -191,12 +190,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; maxGrad = 0; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + for(const Values::ConstKeyValuePair& key_value: inverseRot) { Key key = key_value.key; //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; Vector gradKey = Vector3::Zero(); // collect the gradient for each edge incident on key - BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){ + for(const size_t& factorId: adjEdgesMap.at(key)){ Rot3 Rij = factorId2RotMap.at(factorId); Rot3 Ri = inverseRot.at(key); if( key == (pose3Graph.at(factorId))->keys()[0] ){ @@ -236,7 +235,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // Return correct rotations const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior Values estimateRot; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + for(const Values::ConstKeyValuePair& key_value: inverseRot) { Key key = key_value.key; if (key != keyAnchor) { const Rot3& R = inverseRot.at(key); @@ -252,7 +251,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const /* ************************************************************************* */ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ size_t factorId = 0; - BOOST_FOREACH(const boost::shared_ptr& factor, pose3Graph) { + for(const boost::shared_ptr& factor: pose3Graph) { boost::shared_ptr > pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ @@ -321,7 +320,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // put into Values structure Values initialPose; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ + for(const Values::ConstKeyValuePair& key_value: initialRot){ Key key = key_value.key; const Rot3& rot = initialRot.at(key); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); @@ -330,7 +329,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); initialPose.insert(keyAnchor, Pose3()); - pose3graph.add(PriorFactor(keyAnchor, Pose3(), priorModel)); + pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; @@ -346,7 +345,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // put into Values structure Values estimate; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) { + for(const Values::ConstKeyValuePair& key_value: GNresult) { Key key = key_value.key; if (key != keyAnchor) { const Pose3& pose = GNresult.at(key); @@ -391,7 +390,7 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); - // BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { + // for(const Values::ConstKeyValuePair& key_value: orientations) { // Key key = key_value.key; // if (key != keyAnchor) { // const Point3& pos = givenGuess.at(key).translation(); diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f9f258055..d09627b77 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -44,7 +44,7 @@ public: Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); - BOOST_FOREACH(const Key& key, keys) + for(const Key& key: keys) QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index dabb7600d..8afe0bcbf 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -45,7 +45,7 @@ public: Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); - BOOST_FOREACH(const Key& key, keys) + for(const Key& key: keys) QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } @@ -67,7 +67,7 @@ public: size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); - // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + // for(const KeyMatrixZD& it: Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); std::vector QF; diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index dee8e925f..d13c28e11 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -56,7 +56,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Default constructor - GenericProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + GenericProjectionFactor() : + measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { + } /** * Constructor @@ -108,7 +110,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "GenericProjectionFactor, z = "; - measured_.print(); + traits::Print(measured_); if(this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); Base::print("", keyFormatter); @@ -119,7 +121,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); } @@ -134,16 +136,14 @@ namespace gtsam { PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); *H1 = *H1 * H0; - return reprojectionError.vector(); + return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); - return reprojectionError.vector(); + return camera.project(point, H1, H2, boost::none) - measured_; } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); - return reprojectionError.vector(); + return camera.project(point, H1, H2, boost::none) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 966ade343..68b42f210 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -84,7 +84,7 @@ public: * each degree of freedom. */ ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2) - : Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma), + : Base(noiseModel::Isotropic::Sigma(traits::dimension, sigma), globalKey, transKey, localKey) {} virtual ~ReferenceFrameFactor(){} @@ -100,7 +100,7 @@ public: boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) - *Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension); + *Dlocal = -1* Matrix::Identity(traits::dimension, traits::dimension); return traits::Local(local,newlocal); } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index c713eff72..7c481d0c8 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -10,7 +10,6 @@ #include #include #include -#include #include namespace gtsam { @@ -131,7 +130,7 @@ public: // Do the Schur complement SymmetricBlockMatrix augmentedHessian = // Set::SchurComplement(FBlocks_, E_, b_); - return augmentedHessian.matrix(); + return augmentedHessian.selfadjointView(); } /// *Compute* full information matrix diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 07bd20d4d..1453d80a7 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -161,7 +161,7 @@ public: /// Collect all cameras: important that in key order virtual Cameras cameras(const Values& values) const { Cameras cameras; - BOOST_FOREACH(const Key& k, this->keys_) + for(const Key& k: this->keys_) cameras.push_back(values.at(k)); return cameras; } @@ -189,7 +189,7 @@ public: bool areMeasurementsEqual = true; for (size_t i = 0; i < measured_.size(); i++) { - if (this->measured_.at(i).equals(e->measured_.at(i), tol) == false) + if (traits::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false) areMeasurementsEqual = false; break; } @@ -202,7 +202,7 @@ public: boost::optional Fs = boost::none, // boost::optional E = boost::none) const { Vector ue = cameras.reprojectionError(point, measured_, Fs, E); - if(body_P_sensor_){ + if(body_P_sensor_ && Fs){ for(size_t i=0; i < Fs->size(); i++){ Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); Matrix J(6, 6); @@ -210,9 +210,17 @@ public: Fs->at(i) = Fs->at(i) * J; } } + correctForMissingMeasurements(cameras, ue, Fs, E); return ue; } + /** + * This corrects the Jacobians for the case in which some pixel measurement is missing (nan) + * In practice, this does not do anything in the monocular case, but it is implemented in the stereo version + */ + virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional Fs = boost::none, + boost::optional E = boost::none) const {} + /** * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] * Noise model applied diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h new file mode 100644 index 000000000..761703f8b --- /dev/null +++ b/gtsam/slam/SmartFactorParams.h @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartFactorParams + * @brief Collect common parameters for SmartProjection and SmartStereoProjection factors + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors + */ +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; + +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + +/* + * Parameters for the smart (stereo) projection factors + */ +struct GTSAM_EXPORT SmartProjectionParams { + + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor + + /// @name Parameters governing the triangulation + /// @{ + TriangulationParameters triangulation; + double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + // Constructor + SmartProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false, double retriangulationTh = 1e-5) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + retriangulationTh), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartProjectionParams() { + } + + void print(const std::string& str = "") const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + // get class variables + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + double getRetriangulationThreshold() const { + return retriangulationThreshold; + } + // set class variables + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRetriangulationThreshold(double retriangulationTh) { + retriangulationThreshold = retriangulationTh; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(linearizationMode); + ar & BOOST_SERIALIZATION_NVP(degeneracyMode); + ar & BOOST_SERIALIZATION_NVP(triangulation); + ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); + ar & BOOST_SERIALIZATION_NVP(throwCheirality); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality); + } +}; + +} // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 01f89e526..840ecbc4d 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -31,103 +32,6 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to -enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD -}; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY -}; - -/* - * Parameters for the smart projection factors - */ -struct GTSAM_EXPORT SmartProjectionParams { - - LinearizationMode linearizationMode; ///< How to linearize the factor - DegeneracyMode degeneracyMode; ///< How to linearize the factor - - /// @name Parameters governing the triangulation - /// @{ - TriangulationParameters triangulation; - double retriangulationThreshold; ///< threshold to decide whether to re-triangulate - /// @} - - /// @name Parameters governing how triangulation result is treated - /// @{ - bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) - bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - - // Constructor - SmartProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false) : - linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - 1e-5), throwCheirality(throwCheirality), verboseCheirality( - verboseCheirality) { - } - - virtual ~SmartProjectionParams() { - } - - void print(const std::string& str) const { - std::cout << "linearizationMode: " << linearizationMode << "\n"; - std::cout << " degeneracyMode: " << degeneracyMode << "\n"; - std::cout << triangulation << std::endl; - } - - LinearizationMode getLinearizationMode() const { - return linearizationMode; - } - DegeneracyMode getDegeneracyMode() const { - return degeneracyMode; - } - TriangulationParameters getTriangulationParameters() const { - return triangulation; - } - bool getVerboseCheirality() const { - return verboseCheirality; - } - bool getThrowCheirality() const { - return throwCheirality; - } - void setLinearizationMode(LinearizationMode linMode) { - linearizationMode = linMode; - } - void setDegeneracyMode(DegeneracyMode degMode) { - degeneracyMode = degMode; - } - void setRankTolerance(double rankTol) { - triangulation.rankTolerance = rankTol; - } - void setEnableEPI(bool enableEPI) { - triangulation.enableEPI = enableEPI; - } - void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { - triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; - } - void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { - triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; - } - -private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(linearizationMode); - ar & BOOST_SERIALIZATION_NVP(degeneracyMode); - ar & BOOST_SERIALIZATION_NVP(triangulation); - ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); - ar & BOOST_SERIALIZATION_NVP(throwCheirality); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality); - } -}; - /** * SmartProjectionFactor: triangulates point and keeps an estimate of it around. * This factor operates with monocular cameras, where a camera is expected to @@ -290,9 +194,9 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian - BOOST_FOREACH(Matrix& m, Gs) + for(Matrix& m: Gs) m = Matrix::Zero(Base::Dim, Base::Dim); - BOOST_FOREACH(Vector& v, gs) + for(Vector& v: gs) v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, Gs, gs, 0.0); @@ -499,8 +403,8 @@ public: return Base::totalReprojectionError(cameras, *result_); else if (params_.degeneracyMode == HANDLE_INFINITY) { // Otherwise, manage the exceptions with rotation-only factors - const Point2& z0 = this->measured_.at(0); - Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0); + Unit3 backprojected = cameras.front().backprojectPointAtInfinity( + this->measured_.at(0)); return Base::totalReprojectionError(cameras, backprojected); } else // if we don't want to manage the exceptions we discard the factor @@ -528,19 +432,19 @@ public: } /// Is result valid? - bool isValid() const { - return result_; - } + bool isValid() const { return result_.valid(); } /** return the degenerate state */ - bool isDegenerate() const { - return result_.degenerate(); - } + bool isDegenerate() const { return result_.degenerate(); } /** return the cheirality status flag */ - bool isPointBehindCamera() const { - return result_.behindCamera(); - } + bool isPointBehindCamera() const { return result_.behindCamera(); } + + /** return the outlier state */ + bool isOutlier() const { return result_.outlier(); } + + /** return the farPoint state */ + bool isFarPoint() const { return result_.farPoint(); } private: diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 455e0de87..bb261a9c4 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -123,7 +123,7 @@ public: */ typename Base::Cameras cameras(const Values& values) const { typename Base::Cameras cameras; - BOOST_FOREACH(const Key& k, this->keys_) { + for(const Key& k: this->keys_) { Pose3 pose = values.at(k); if (Base::body_P_sensor_) pose = pose.compose(*(Base::body_P_sensor_)); diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index e97cd2730..811d92fbc 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -78,12 +78,11 @@ public: bool verboseCheirality = false) : Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( throwCheirality), verboseCheirality_(verboseCheirality) { - if (model && model->dim() != Measurement::dimension) + if (model && model->dim() != traits::dimension) throw std::invalid_argument( "TriangulationFactor must be created with " - + boost::lexical_cast((int) Measurement::dimension) + + boost::lexical_cast((int) traits::dimension) + "-dimensional noise model."); - } /** Virtual destructor */ @@ -105,7 +104,7 @@ public: DefaultKeyFormatter) const { std::cout << s << "TriangulationFactor,"; camera_.print("camera"); - measured_.print("z"); + traits::Print(measured_, "z"); Base::print("", keyFormatter); } @@ -113,25 +112,24 @@ public: virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol) - && this->measured_.equals(e->measured_, tol); + && traits::Equals(this->measured_, e->measured_, tol); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Measurement error(camera_.project2(point, boost::none, H2) - measured_); - return error.vector(); + return traits::Local(measured_, camera_.project2(point, boost::none, H2)); } catch (CheiralityException& e) { if (H2) - *H2 = Matrix::Zero(Measurement::dimension, 3); + *H2 = Matrix::Zero(traits::dimension, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" << std::endl; if (throwCheirality_) throw e; - return Eigen::Matrix::Constant(2.0 * camera_.calibration().fx()); + return Eigen::Matrix::dimension,1>::Constant(2.0 * camera_.calibration().fx()); } } @@ -153,14 +151,14 @@ public: // Allocate memory for Jacobian factor, do only once if (Ab.rows() == 0) { std::vector dimensions(1, 3); - Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true); - A.resize(Measurement::dimension,3); - b.resize(Measurement::dimension); + Ab = VerticalBlockMatrix(dimensions, traits::dimension, true); + A.resize(traits::dimension,3); + b.resize(traits::dimension); } // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project2(point, boost::none, A) - measured_).vector(); + b = traits::Local(camera_.project2(point, boost::none, A), measured_); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b2a90cb84..dc25026d2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include @@ -68,8 +67,8 @@ string findExampleDataFile(const string& name) { namesToSearch.push_back(name + ".out"); // Find first name that exists - BOOST_FOREACH(const fs::path& root, rootsToSearch) { - BOOST_FOREACH(const fs::path& name, namesToSearch) { + for(const fs::path& root: rootsToSearch) { + for(const fs::path& name: namesToSearch) { if (fs::is_regular_file(root / name)) return (root / name).string(); } @@ -366,7 +365,7 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { + for(const Values::ConstKeyValuePair& key_value: config) { const Pose2& pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; @@ -375,7 +374,7 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, // save edges Matrix R = model->R(); Matrix RR = trans(R) * R; //prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr factor_, graph) { + for(boost::shared_ptr factor_: graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (!factor) @@ -413,13 +412,13 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, // save 2D & 3D poses Values::ConstFiltered viewPose2 = estimate.filter(); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose2) { + for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose2) { stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " << key_value.value.y() << " " << key_value.value.theta() << endl; } Values::ConstFiltered viewPose3 = estimate.filter(); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose3) { + for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose3) { Point3 p = key_value.value.translation(); Rot3 R = key_value.value.rotation(); stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() @@ -428,7 +427,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } // save edges (2D or 3D) - BOOST_FOREACH(boost::shared_ptr factor_, graph) { + for(boost::shared_ptr factor_: graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (factor){ @@ -898,7 +897,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values initialCamerasEstimate(const SfM_data& db) { Values initial; size_t i = 0; // NO POINTS: j = 0; - BOOST_FOREACH(const SfM_Camera& camera, db.cameras) + for(const SfM_Camera& camera: db.cameras) initial.insert(i++, camera); return initial; } @@ -906,9 +905,9 @@ Values initialCamerasEstimate(const SfM_data& db) { Values initialCamerasAndPointsEstimate(const SfM_data& db) { Values initial; size_t i = 0, j = 0; - BOOST_FOREACH(const SfM_Camera& camera, db.cameras) + for(const SfM_Camera& camera: db.cameras) initial.insert((i++), camera); - BOOST_FOREACH(const SfM_Track& track, db.tracks) + for(const SfM_Track& track: db.tracks) initial.insert(P(j++), track.p); return initial; } diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index e81c76bd6..f9f7125cc 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -40,6 +40,14 @@ inline Point3_ transform_from(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transform_from, p); } +inline Point3_ rotate(const Rot3_& x, const Point3_& p) { + return Point3_(x, &Rot3::rotate, p); +} + +inline Point3_ unrotate(const Rot3_& x, const Point3_& p) { + return Point3_(x, &Rot3::unrotate, p); +} + // Projection typedef Expression Cal3_S2_; @@ -58,40 +66,37 @@ inline Point2_ project(const Unit3_& p_cam) { namespace internal { // Helper template for project2 expression below -template -Point2 project4(const CAMERA& camera, const POINT& p, - OptionalJacobian<2, CAMERA::dimension> Dcam, - OptionalJacobian<2, FixedDimension::value> Dpoint) { +template +Point2 project4(const CAMERA& camera, const POINT& p, OptionalJacobian<2, CAMERA::dimension> Dcam, + OptionalJacobian<2, FixedDimension::value> Dpoint) { return camera.project2(p, Dcam, Dpoint); } } -template -Point2_ project2(const Expression& camera_, - const Expression& p_) { +template +Point2_ project2(const Expression& camera_, const Expression& p_) { return Point2_(internal::project4, camera_, p_); } namespace internal { // Helper template for project3 expression below -template +template inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, - OptionalJacobian<2, 5> Dcal) { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, + OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } } -template +template inline Point2_ project3(const Pose3_& x, const Expression& p, - const Expression& K) { + const Expression& K) { return Point2_(internal::project6, x, p, K); } -template +template Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } -} // \namespace gtsam - +} // \namespace gtsam diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index fa6fce37a..e1cf9cea2 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -82,7 +82,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, thetaToRootMap.insert(pair(keyAnchor, 0.0)); // for all nodes in the tree - BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { + for(const key2doubleMap::value_type& it: deltaThetaMap) { // compute the orientation wrt root Key nodeKey = it.first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, @@ -101,7 +101,7 @@ void getSymbolicGraph( // Get keys for which you want the orientation size_t id = 0; // Loop over the factors - BOOST_FOREACH(const boost::shared_ptr& factor, g) { + for(const boost::shared_ptr& factor: g) { if (factor->keys().size() == 2) { Key key1 = factor->keys()[0]; Key key2 = factor->keys()[1]; @@ -167,14 +167,14 @@ GaussianFactorGraph buildLinearOrientationGraph( noiseModel::Diagonal::shared_ptr model_deltaTheta; // put original measurements in the spanning tree - BOOST_FOREACH(const size_t& factorId, spanningTreeIds) { + for(const size_t& factorId: spanningTreeIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta); } // put regularized measurements in the chordsIds - BOOST_FOREACH(const size_t& factorId, chordsIds) { + for(const size_t& factorId: chordsIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); @@ -199,7 +199,7 @@ static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { gttic(lago_buildPose2graph); NonlinearFactorGraph pose2Graph; - BOOST_FOREACH(const boost::shared_ptr& factor, graph) { + for(const boost::shared_ptr& factor: graph) { // recast to a between on Pose2 boost::shared_ptr > pose2Between = @@ -226,7 +226,7 @@ static PredecessorMap findOdometricPath( Key minKey = keyAnchor; // this initialization does not matter bool minUnassigned = true; - BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph) { + for(const boost::shared_ptr& factor: pose2Graph) { Key key1 = std::min(factor->keys()[0], factor->keys()[1]); Key key2 = std::max(factor->keys()[0], factor->keys()[1]); @@ -299,7 +299,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, GaussianFactorGraph linearPose2graph; // We include the linear version of each between factor - BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { + for(const boost::shared_ptr& factor: pose2graph) { boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); @@ -346,7 +346,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, // put into Values structure Values initialGuessLago; - BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { + for(const VectorValues::value_type& it: posesLago) { Key key = it.first; if (key != keyAnchor) { const Vector& poseVector = it.second; @@ -383,7 +383,7 @@ Values initialize(const NonlinearFactorGraph& graph, VectorValues orientations = initializeOrientations(graph); // for all nodes in the tree - BOOST_FOREACH(const VectorValues::value_type& it, orientations) { + for(const VectorValues::value_type& it: orientations) { Key key = it.first; if (key != keyAnchor) { const Pose2& pose = initialGuess.at(key); diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 1dc897599..e34e13279 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -64,19 +64,14 @@ TEST( AntiFactor, NegativeHessian) GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(values); HessianFactor::shared_ptr antiHessian = boost::dynamic_pointer_cast(antiGaussian); + Matrix expected_information = -originalHessian->information(); + Matrix actual_information = antiHessian->information(); + EXPECT(assert_equal(expected_information, actual_information)); + + Vector expected_linear = -originalHessian->linearTerm(); + Vector actual_linear = antiHessian->linearTerm(); + EXPECT(assert_equal(expected_linear, actual_linear)); - // Compare Hessian blocks - size_t variable_count = originalFactor->size(); - for(size_t i = 0; i < variable_count; ++i){ - for(size_t j = i; j < variable_count; ++j){ - Matrix expected_G = -Matrix(originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j)); - Matrix actual_G = antiHessian->info(antiHessian->begin()+i, antiHessian->begin()+j); - CHECK(assert_equal(expected_G, actual_G, 1e-5)); - } - Vector expected_g = -originalHessian->linearTerm(originalHessian->begin()+i); - Vector actual_g = antiHessian->linearTerm(antiHessian->begin()+i); - CHECK(assert_equal(expected_g, actual_g, 1e-5)); - } double expected_f = -originalHessian->constantTerm(); double actual_f = antiHessian->constantTerm(); EXPECT_DOUBLES_EQUAL(expected_f, actual_f, 1e-5); @@ -95,8 +90,8 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; - graph.push_back(PriorFactor(1, pose1, sigma)); - graph.push_back(BetweenFactor(1, 2, pose1.between(pose2), sigma)); + graph.emplace_shared >(1, pose1, sigma); + graph.emplace_shared >(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 1dd4e8abc..b3e208b91 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -112,18 +112,18 @@ TEST( dataSet, readG2o) noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -164,27 +164,27 @@ TEST( dataSet, readG2o3D) Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); Point3 p12 = Point3(0.523923, 0.776654, 0.326659); Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); + expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); Point3 p23 = Point3(0.910927, 0.055169, -0.411761); Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); + expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); Point3 p34 = Point3(0.775288, 0.228798, -0.596923); Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); + expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); + expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); + expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -224,7 +224,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); } @@ -242,18 +242,18 @@ TEST( dataSet, readG2oHuber) SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -270,18 +270,18 @@ TEST( dataSet, readG2oTukey) SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 8ac6c48b6..d33551edf 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -178,7 +178,7 @@ TEST (EssentialMatrixFactor, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1)); + graph.emplace_shared(1, pA(i), pB(i), model1); // Check error at ground truth Values truth; @@ -222,8 +222,7 @@ TEST (EssentialMatrixFactor2, factor) { // Check evaluation Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); const Point2 pi = PinholeBase::Project(P2); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -252,7 +251,7 @@ TEST (EssentialMatrixFactor2, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2)); + graph.emplace_shared(100, i, pA(i), pB(i), model2); // Check error at ground truth Values truth; @@ -296,8 +295,7 @@ TEST (EssentialMatrixFactor3, factor) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -325,7 +323,7 @@ TEST (EssentialMatrixFactor3, minimization) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) // but now we specify the rotation bRc - graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2)); + graph.emplace_shared(100, i, pA(i), pB(i), cRb, model2); // Check error at ground truth Values truth; @@ -393,7 +391,7 @@ TEST (EssentialMatrixFactor, extraMinimization) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K)); + graph.emplace_shared(1, pA(i), pB(i), model1, K); // Check error at ground truth Values truth; @@ -438,8 +436,7 @@ TEST (EssentialMatrixFactor2, extraTest) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -468,7 +465,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); + graph.emplace_shared(100, i, pA(i), pB(i), model2, K); // Check error at ground truth Values truth; @@ -507,8 +504,7 @@ TEST (EssentialMatrixFactor3, extraTest) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 48e2e8b2f..8e0b5ad8f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -368,9 +368,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; @@ -385,12 +385,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.)); + graph.emplace_shared< + PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), GeneralCamera()); @@ -413,15 +413,15 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.push_back( - PriorFactor(X(0), CalibratedCamera(), - noiseModel::Isotropic::Sigma(6, 1.))); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + PriorFactor >(X(0), CalibratedCamera(), + noiseModel::Isotropic::Sigma(6, 1.)); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.)); + graph.emplace_shared< + PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), CalibratedCamera()); @@ -465,7 +465,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) { } // Now loop over all these noise models - BOOST_FOREACH(SharedNoiseModel model, models) { + for(SharedNoiseModel model: models) { Projection factor(measurement, model, X(1), L(1)); // Test linearize diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9fda7d745..d8ac0aa5b 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -369,9 +369,9 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 7db71d8ce..f8157c116 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -285,7 +285,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { Values::shared_ptr expected; boost::tie(gmatlab, expected) = readG2o(matlabFile); - BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ + for(const Values::KeyValuePair& key_val: *expected){ Key k = key_val.key; EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-5)); } @@ -311,7 +311,7 @@ TEST( Lago, largeGraphNoisy ) { Values::shared_ptr expected; boost::tie(gmatlab, expected) = readG2o(matlabFile); - BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ + for(const Values::KeyValuePair& key_val: *expected){ Key k = key_val.key; EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-2)); } diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index f1e830d03..ea42a1ecd 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -24,7 +24,6 @@ #include #include -#include #include using namespace boost::assign; diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index f955aa191..24b818835 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -136,15 +136,15 @@ TEST( ReferenceFrameFactor, converge_trans ) { // Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails NonlinearFactorGraph graph; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared(lB2, tA1, lA2); // hard constraints on points double error_gain = 1000.0; - graph.push_back(NonlinearEquality(lA1, local1, error_gain)); - graph.push_back(NonlinearEquality(lA2, local2, error_gain)); - graph.push_back(NonlinearEquality(lB1, global1, error_gain)); - graph.push_back(NonlinearEquality(lB2, global2, error_gain)); + graph.emplace_shared >(lA1, local1, error_gain); + graph.emplace_shared >(lA2, local2, error_gain); + graph.emplace_shared >(lB1, global1, error_gain); + graph.emplace_shared >(lB2, global2, error_gain); // create initial estimate Values init; @@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(NonlinearEquality(lB1, global, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lB1, global, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; @@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(NonlinearEquality(lA1, local, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lA1, local, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 9bb296444..caf3d31df 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -89,9 +89,9 @@ TEST (RotateFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; Model model = noiseModel::Isotropic::Sigma(3, 0.01); - graph.add(RotateFactor(1, i1Ri2, c1Zc2, model)); - graph.add(RotateFactor(1, i2Ri3, c2Zc3, model)); - graph.add(RotateFactor(1, i3Ri4, c3Zc4, model)); + graph.emplace_shared(1, i1Ri2, c1Zc2, model); + graph.emplace_shared(1, i2Ri3, c2Zc3, model); + graph.emplace_shared(1, i3Ri4, c3Zc4, model); // Check error at ground truth Values truth; @@ -162,9 +162,9 @@ TEST (RotateDirectionsFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; Model model = noiseModel::Isotropic::Sigma(2, 0.01); - graph.add(RotateDirectionsFactor(1, p1, z1, model)); - graph.add(RotateDirectionsFactor(1, p2, z2, model)); - graph.add(RotateDirectionsFactor(1, p3, z3, model)); + graph.emplace_shared(1, p1, z1, model); + graph.emplace_shared(1, p2, z2, model); + graph.emplace_shared(1, p3, z3, model); // Check error at ground truth Values truth; diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp new file mode 100644 index 000000000..66b236ac2 --- /dev/null +++ b/gtsam/slam/tests/testSlamExpressions.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSlamExpressions.cpp + * @author Frank Dellaert + * @brief test expressions for SLAM + */ + +#include +#include +#include + +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(SlamExpressions, project2) { + typedef Expression CalibratedCamera_; + + Rot3_ rot3_expr('r', 0); // unknown rotation ! + Point3_ t_expr(Point3(1, 2, 3)); // known translation + Pose3_ pose_expr(&Pose3::Create, rot3_expr, t_expr); + CalibratedCamera_ camera_expr(&CalibratedCamera::Create, pose_expr); + Point3_ point3_expr('p', 12); // unknown 3D point with index 12, for funsies + Point2_ point2_expr = project2(camera_expr, point3_expr); + + // Set the linearization point + Values values; + values.insert(Symbol('r', 0), Rot3()); + values.insert(Symbol('p', 12), Point3(4, 5, 6)); + + EXPECT_CORRECT_EXPRESSION_JACOBIANS(point2_expr, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 96052bd0f..f69f4c113 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -55,8 +55,8 @@ struct traits : public Testable {}; TEST(SmartFactorBase, Pinhole) { PinholeFactor f= PinholeFactor(unit2); - f.add(Point2(), 1); - f.add(Point2(), 2); + f.add(Point2(0,0), 1); + f.add(Point2(0,0), 2); EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 7eefb2398..4feeadc86 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -209,8 +209,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); // Create initial estimate Values initial; @@ -300,20 +300,14 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { SfM_Track track1; for (size_t i = 0; i < 3; ++i) { - SfM_Measurement measures; - measures.first = i + 1; // cameras are from 1 to 3 - measures.second = measurements_cam1.at(i); - track1.measurements.push_back(measures); + track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); } SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); smartFactor1->add(track1); SfM_Track track2; for (size_t i = 0; i < 3; ++i) { - SfM_Measurement measures; - measures.first = i + 1; // cameras are from 1 to 3 - measures.second = measurements_cam2.at(i); - track2.measurements.push_back(measures); + track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); } SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); smartFactor2->add(track2); @@ -327,8 +321,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -404,8 +398,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { graph.push_back(smartFactor3); graph.push_back(smartFactor4); graph.push_back(smartFactor5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -482,8 +476,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -558,8 +552,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -651,8 +645,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation Vector e1 = sfm1.evaluateError(values.at(c1), values.at(l1)); Vector e2 = sfm2.evaluateError(values.at(c2), values.at(l1)); - double actualError = 0.5 - * (e1.norm() * e1.norm() + e2.norm() * e2.norm()); + double actualError = 0.5 * (e1.squaredNorm() + e2.squaredNorm()); double actualErrorGraph = generalGraph.error(values); DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index ace75f80f..71ee79d86 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -81,6 +81,17 @@ TEST( SmartProjectionPoseFactor, Constructor4) { factor1.add(measurement1, x1); } +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, params) { + using namespace vanillaPose; + SmartProjectionParams params; + double rt = params.getRetriangulationThreshold(); + EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7); + params.setRetriangulationThreshold(1e-3); + rt = params.getRetriangulationThreshold(); + EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); +} + /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; @@ -251,8 +262,8 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); - graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + graph.emplace_shared >(x1, bodyPose1, noisePrior); + graph.emplace_shared >(x2, bodyPose2, noisePrior); // Check errors at ground truth poses Values gtValues; @@ -308,8 +319,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Values groundTruth; groundTruth.insert(x1, cam1.pose()); @@ -536,8 +547,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -603,8 +614,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -664,8 +675,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -736,8 +747,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Values values; values.insert(x1, cam1.pose()); @@ -789,8 +800,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -819,30 +830,21 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { NonlinearFactorGraph graph; // Project three landmarks into three cameras - graph.push_back( - ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2)); + graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); - graph.push_back( - ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2)); + graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); - graph.push_back( - ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); + graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.emplace_shared >(x1, level_pose, noisePrior); + graph.emplace_shared >(x2, pose_right, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -989,11 +991,9 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -1011,6 +1011,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { + // this test considers a condition in which the cheirality constraint is triggered using namespace vanillaPose; vector views; @@ -1057,11 +1058,9 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1085,8 +1084,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION EXPECT(assert_equal(Pose3(values.at(x3).rotation(), Point3(0,0,1)), result.at(x3))); +#else + // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation + // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) + EXPECT(assert_equal(pose3, result.at(x3),1e-3)); +#endif } /* *************************************************************************/ @@ -1275,8 +1280,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1351,11 +1356,9 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 45b978c27..c802603db 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -185,11 +185,11 @@ TEST( StereoFactor, singlePoint) { NonlinearFactorGraph graph; - graph.push_back(NonlinearEquality(X(1), camera1)); + graph.emplace_shared >(X(1), camera1); StereoPoint2 measurement(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.push_back(GenericStereoFactor(measurement, model, X(1), L(1), K)); + graph.emplace_shared >(measurement, model, X(1), L(1), K); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/symbolic/SymbolicBayesNet.cpp b/gtsam/symbolic/SymbolicBayesNet.cpp index c5a04eb0d..038ccecbf 100644 --- a/gtsam/symbolic/SymbolicBayesNet.cpp +++ b/gtsam/symbolic/SymbolicBayesNet.cpp @@ -20,6 +20,7 @@ #include #include +#include #include namespace gtsam { @@ -39,11 +40,11 @@ namespace gtsam { std::ofstream of(s.c_str()); of << "digraph G{\n"; - BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) { + for (auto conditional: boost::adaptors::reverse(*this)) { SymbolicConditional::Frontals frontals = conditional->frontals(); Key me = frontals.front(); SymbolicConditional::Parents parents = conditional->parents(); - BOOST_FOREACH(Key p, parents) + for(Key p: parents) of << p << "->" << me << std::endl; } diff --git a/gtsam/symbolic/SymbolicBayesTree.cpp b/gtsam/symbolic/SymbolicBayesTree.cpp index 8797ba363..aab626837 100644 --- a/gtsam/symbolic/SymbolicBayesTree.cpp +++ b/gtsam/symbolic/SymbolicBayesTree.cpp @@ -16,8 +16,6 @@ * @author Richard Roberts */ -#include - #include #include #include diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index f1e2b48c2..47bb06515 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -24,7 +24,6 @@ #include #include -#include #include #include @@ -43,12 +42,12 @@ namespace gtsam // Gather all keys KeySet allKeys; - BOOST_FOREACH(const boost::shared_ptr& factor, factors) { + for(const boost::shared_ptr& factor: factors) { allKeys.insert(factor->begin(), factor->end()); } // Check keys - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { if(allKeys.find(key) == allKeys.end()) throw std::runtime_error("Requested to eliminate a key that is not in the factors"); } diff --git a/gtsam/symbolic/SymbolicJunctionTree.cpp b/gtsam/symbolic/SymbolicJunctionTree.cpp index 00a52497b..261f682b0 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.cpp +++ b/gtsam/symbolic/SymbolicJunctionTree.cpp @@ -23,7 +23,7 @@ namespace gtsam { // Instantiate base class - template class ClusterTree; + template class EliminatableClusterTree; template class JunctionTree; /* ************************************************************************* */ diff --git a/gtsam/symbolic/SymbolicJunctionTree.h b/gtsam/symbolic/SymbolicJunctionTree.h index 709488cbf..fdc5450b3 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.h +++ b/gtsam/symbolic/SymbolicJunctionTree.h @@ -26,7 +26,7 @@ namespace gtsam { class SymbolicEliminationTree; /** - * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with + * A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with * the additional property that it represents the clique tree associated with a Bayes net. * * In GTSAM a junction tree is an intermediate data structure in multifrontal diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 82d8fb3ec..1b84e291f 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -213,7 +213,7 @@ void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayes if (subtree) { cliques.push_back(subtree); // Recursive call over all child cliques - BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children) { + for(SymbolicBayesTree::sharedClique& childClique: subtree->children) { getAllCliques(childClique,cliques); } } @@ -241,7 +241,7 @@ TEST( BayesTree, shortcutCheck ) SymbolicBayesTree::Cliques allCliques; getAllCliques(rootClique,allCliques); - BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { + for(SymbolicBayesTree::sharedClique& clique: allCliques) { //clique->print("Clique#"); SymbolicBayesNet bn = clique->shortcut(rootClique); //bn.print("Shortcut:\n"); @@ -250,13 +250,13 @@ TEST( BayesTree, shortcutCheck ) // Check if all the cached shortcuts are cleared rootClique->deleteCachedShortcuts(); - BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { + for(SymbolicBayesTree::sharedClique& clique: allCliques) { bool notCleared = clique->cachedSeparatorMarginal().is_initialized(); CHECK( notCleared == false); } EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); -// BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { +// for(SymbolicBayesTree::sharedClique& clique: allCliques) { // clique->print("Clique#"); // if(clique->cachedShortcut()){ // bn = clique->cachedShortcut().get(); @@ -726,6 +726,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { EXPECT(assert_equal(expected, actual)); } +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION // METIS { Ordering ordering = Ordering::Create(Ordering::METIS, sfg); @@ -759,6 +760,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); } +#endif } /* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp index d192a6064..994e9b107 100644 --- a/gtsam/symbolic/tests/testSymbolicISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -15,14 +15,13 @@ * @author Michael Kaess */ -#include -#include // for operator += -using namespace boost::assign; +#include +#include #include -#include -#include +#include // for operator += +using namespace boost::assign; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index baa6540e7..f544366b2 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -395,7 +395,7 @@ namespace gtsam { }; // const_iterator - // to make BTree work with BOOST_FOREACH + // to make BTree work with range-based for // We do *not* want a non-const iterator typedef const_iterator iterator; diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index c51d6003c..ba545de35 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include #include @@ -58,14 +57,14 @@ public: // constructor with a list of unconnected keys DSF(const std::list& keys) : Tree() { - BOOST_FOREACH(const KEY& key, keys) + for(const KEY& key: keys) *this = this->add(key, key); } // constructor with a set of unconnected keys DSF(const std::set& keys) : Tree() { - BOOST_FOREACH(const KEY& key, keys) + for(const KEY& key: keys) *this = this->add(key, key); } @@ -109,7 +108,7 @@ public: // create a new singleton with a list of fully connected keys Self makeList(const std::list& keys) const { Self t = *this; - BOOST_FOREACH(const KEY& key, keys) + for(const KEY& key: keys) t = t.makePair(key, keys.front()); return t; } @@ -117,7 +116,7 @@ public: // return a dsf in which all find_set operations will be O(1) due to path compression. DSF flatten() const { DSF t = *this; - BOOST_FOREACH(const KeyLabel& pair, (Tree)t) + for(const KeyLabel& pair: (Tree)t) t.findSet_(pair.first); return t; } @@ -125,7 +124,7 @@ public: // maps f over all keys, must be invertible DSF map(boost::function func) const { DSF t; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) + for(const KeyLabel& pair: (Tree)*this) t = t.add(func(pair.first), func(pair.second)); return t; } @@ -133,7 +132,7 @@ public: // return the number of sets size_t numSets() const { size_t num = 0; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) + for(const KeyLabel& pair: (Tree)*this) if (pair.first == pair.second) num++; return num; @@ -147,7 +146,7 @@ public: // return all sets, i.e. a partition of all elements std::map sets() const { std::map sets; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) + for(const KeyLabel& pair: (Tree)*this) sets[findSet(pair.second)].insert(pair.first); return sets; } @@ -155,7 +154,7 @@ public: // return a partition of the given elements {keys} std::map partition(const std::list& keys) const { std::map partitions; - BOOST_FOREACH(const KEY& key, keys) + for(const KEY& key: keys) partitions[findSet(key)].insert(key); return partitions; } @@ -163,7 +162,7 @@ public: // get the nodes in the tree with the given label Set set(const KEY& label) const { Set set; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) { + for(const KeyLabel& pair: (Tree)*this) { if (pair.second == label || findSet(pair.second) == label) set.insert(pair.first); } @@ -183,7 +182,7 @@ public: // print the object void print(const std::string& name = "DSF") const { std::cout << name << std::endl; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) + for(const KeyLabel& pair: (Tree)*this) std::cout << (std::string) pair.first << " " << (std::string) pair.second << std::endl; } diff --git a/gtsam_unstable/base/tests/testBTree.cpp b/gtsam_unstable/base/tests/testBTree.cpp index 4fb35dca8..bbc22c8e5 100644 --- a/gtsam_unstable/base/tests/testBTree.cpp +++ b/gtsam_unstable/base/tests/testBTree.cpp @@ -17,7 +17,6 @@ */ #include -#include #include // for += using namespace boost::assign; @@ -146,9 +145,9 @@ TEST( BTree, iterating ) CHECK(*(++it) == p5) CHECK((++it)==tree.end()) - // acid iterator test: BOOST_FOREACH + // acid iterator test int sum = 0; - BOOST_FOREACH(const KeyInt& p, tree) + for(const KeyInt& p: tree) sum += p.second; LONGS_EQUAL(15,sum) diff --git a/gtsam_unstable/base/tests/testDSF.cpp b/gtsam_unstable/base/tests/testDSF.cpp index c8828746b..298d439bc 100644 --- a/gtsam_unstable/base/tests/testDSF.cpp +++ b/gtsam_unstable/base/tests/testDSF.cpp @@ -295,7 +295,7 @@ TEST(DSF, mergePairwiseMatches) { // Merge matches DSF dsf(measurements); - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.makeUnionInPlace(m.first,m.second); // Check that sets are merged correctly diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam_unstable/base/tests/testDSFMap.cpp index 69723d99b..9c9143a15 100644 --- a/gtsam_unstable/base/tests/testDSFMap.cpp +++ b/gtsam_unstable/base/tests/testDSFMap.cpp @@ -18,7 +18,6 @@ #include -#include #include #include using namespace boost::assign; @@ -72,7 +71,7 @@ TEST(DSFMap, mergePairwiseMatches) { // Merge matches DSFMap dsf; - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.merge(m.first,m.second); // Each point is now associated with a set, represented by one of its members @@ -102,7 +101,7 @@ TEST(DSFMap, mergePairwiseMatches2) { // Merge matches DSFMap dsf; - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.merge(m.first,m.second); // Check that sets are merged correctly @@ -122,7 +121,7 @@ TEST(DSFMap, sets){ // Merge matches DSFMap dsf; - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.merge(m.first,m.second); map > sets = dsf.sets(); @@ -130,9 +129,9 @@ TEST(DSFMap, sets){ s1 += 1,2,3; s2 += 4,5,6; - /*BOOST_FOREACH(key_pair st, sets){ + /*for(key_pair st: sets){ cout << "Set " << st.first << " :{"; - BOOST_FOREACH(const size_t s, st.second) + for(const size_t s: st.second) cout << s << ", "; cout << "}" << endl; }*/ diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index b230aa570..9e124954f 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -15,7 +15,7 @@ namespace gtsam { /* ************************************************************************* */ AllDiff::AllDiff(const DiscreteKeys& dkeys) : Constraint(dkeys.indices()) { - BOOST_FOREACH(const DiscreteKey& dkey, dkeys) + for(const DiscreteKey& dkey: dkeys) cardinalities_.insert(dkey); } @@ -23,7 +23,7 @@ namespace gtsam { void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { std::cout << s << "AllDiff on "; - BOOST_FOREACH (Key dkey, keys_) + for (Key dkey: keys_) std::cout << formatter(dkey) << " "; std::cout << std::endl; } @@ -31,7 +31,7 @@ namespace gtsam { /* ************************************************************************* */ double AllDiff::operator()(const Values& values) const { std::set < size_t > taken; // record values taken by keys - BOOST_FOREACH(Key dkey, keys_) { + for(Key dkey: keys_) { size_t value = values.at(dkey); // get the value for that key if (taken.count(value)) return 0.0;// check if value alreday taken taken.insert(value);// if not, record it as taken and keep checking @@ -70,7 +70,7 @@ namespace gtsam { // Check all other domains for singletons and erase corresponding values // This is the same as arc-consistency on the equivalent binary constraints bool changed = false; - BOOST_FOREACH(Key k, keys_) + for(Key k: keys_) if (k != j) { const Domain& Dk = domains[k]; if (Dk.isSingleton()) { // check if singleton @@ -88,7 +88,7 @@ namespace gtsam { Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { DiscreteKeys newKeys; // loop over keys and add them only if they do not appear in values - BOOST_FOREACH(Key k, keys_) + for(Key k: keys_) if (values.find(k) == values.end()) { newKeys.push_back(DiscreteKey(k,cardinalities_.at(k))); } @@ -99,7 +99,7 @@ namespace gtsam { Constraint::shared_ptr AllDiff::partiallyApply( const std::vector& domains) const { DiscreteFactor::Values known; - BOOST_FOREACH(Key k, keys_) { + for(Key k: keys_) { const Domain& Dk = domains[k]; if (Dk.isSingleton()) known[k] = Dk.firstValue(); diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index ae716f246..cf5abdcb1 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -8,7 +8,6 @@ #include #include #include -#include using namespace std; @@ -45,7 +44,7 @@ namespace gtsam { changed[v] = false; // loop over all factors/constraints for variable v const VariableIndex::Factors& factors = index[v]; - BOOST_FOREACH(size_t f,factors) { + for(size_t f: factors) { // if not already a singleton if (!domains[v].isSingleton()) { // get the constraint and call its ensureArcConsistency method @@ -85,7 +84,7 @@ namespace gtsam { // TODO: create a new ordering as we go, to ensure a connected graph // KeyOrdering ordering; // vector dkeys; - BOOST_FOREACH(const DiscreteFactor::shared_ptr& f, factors_) { + for(const DiscreteFactor::shared_ptr& f: factors_) { Constraint::shared_ptr constraint = boost::dynamic_pointer_cast(f); if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); Constraint::shared_ptr reduced = constraint->partiallyApply(domains); diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 923365c40..93f5c5d7d 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -54,7 +54,7 @@ namespace gtsam { // /** return product of all factors as a single factor */ // DecisionTreeFactor product() const { // DecisionTreeFactor result; -// BOOST_FOREACH(const sharedFactor& factor, *this) +// for(const sharedFactor& factor: *this) // if (factor) result = (*factor) * result; // return result; // } diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 94c977cb0..72d424baf 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -19,9 +19,9 @@ namespace gtsam { const KeyFormatter& formatter) const { // cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << // formatter(keys_[0]) << ") with values"; -// BOOST_FOREACH (size_t v,values_) cout << " " << v; +// for (size_t v: values_) cout << " " << v; // cout << endl; - BOOST_FOREACH (size_t v,values_) cout << v; + for (size_t v: values_) cout << v; } /* ************************************************************************* */ @@ -50,7 +50,7 @@ namespace gtsam { bool Domain::ensureArcConsistency(size_t j, vector& domains) const { if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); Domain& D = domains[j]; - BOOST_FOREACH(size_t value, values_) + for(size_t value: values_) if (!D.contains(value)) throw runtime_error("Unsatisfiable"); D = *this; return true; @@ -60,9 +60,9 @@ namespace gtsam { bool Domain::checkAllDiff(const vector keys, vector& domains) { Key j = keys_[0]; // for all values in this domain - BOOST_FOREACH(size_t value, values_) { + for(size_t value: values_) { // for all connected domains - BOOST_FOREACH(Key k, keys) + for(Key k: keys) // if any domain contains the value we cannot make this domain singleton if (k!=j && domains[k].contains(value)) goto found; diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 1f22fa0e6..a81048291 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -11,7 +11,6 @@ #include #include -#include #include #include @@ -163,7 +162,7 @@ namespace gtsam { if (!mutexBound) { DiscreteKeys dkeys; - BOOST_FOREACH(const Student& s, students_) + for(const Student& s: students_) dkeys.push_back(s.key_); addAllDiff(dkeys); } else { @@ -182,30 +181,30 @@ namespace gtsam { void Scheduler::print(const string& s) const { cout << s << " Faculty:" << endl; - BOOST_FOREACH(const string& name, facultyName_) + for(const string& name: facultyName_) cout << name << '\n'; cout << endl; cout << s << " Slots:\n"; size_t i = 0; - BOOST_FOREACH(const string& name, slotName_) + for(const string& name: slotName_) cout << i++ << " " << name << endl; cout << endl; cout << "Availability:\n" << available_ << '\n'; cout << s << " Area constraints:\n"; - BOOST_FOREACH(const FacultyInArea::value_type& it, facultyInArea_) + for(const FacultyInArea::value_type& it: facultyInArea_) { cout << setw(12) << it.first << ": "; - BOOST_FOREACH(double v, it.second) + for(double v: it.second) cout << v << " "; cout << '\n'; } cout << endl; cout << s << " Students:\n"; - BOOST_FOREACH (const Student& student, students_) + for (const Student& student: students_) student.print(); cout << endl; diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index 0679dcdfa..e9f63b2d8 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include @@ -303,7 +302,7 @@ void accomodateStudent() { vector slots; slots += 16, 17, 11, 2, 0, 5, 9, 14; vector slotsAvailable(scheduler.nrTimeSlots(), 1.0); - BOOST_FOREACH(size_t s, slots) + for(size_t s: slots) slotsAvailable[s] = 0; scheduler.setSlotsAvailable(slotsAvailable); diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index 3a7ea4d2a..1fc4a1459 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index 0b5238783..95b64f289 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 37a4fe5a4..d2efc3a2d 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -45,7 +45,7 @@ class LoopyBelief { void print(const std::string& s = "") const { cout << s << ":" << endl; star->print("Star graph: "); - BOOST_FOREACH(Key key, correctedBeliefIndices | boost::adaptors::map_keys) { + for(Key key: correctedBeliefIndices | boost::adaptors::map_keys) { cout << "Belief factor index for " << key << ": " << correctedBeliefIndices.at(key) << endl; } @@ -70,7 +70,7 @@ public: /// print void print(const std::string& s = "") const { cout << s << ":" << endl; - BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) { + for(Key key: starGraphs_ | boost::adaptors::map_keys) { starGraphs_.at(key).print((boost::format("Node %d:") % key).str()); } } @@ -83,7 +83,7 @@ public: DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph()); std::map > allMessages; // Eliminate each star graph - BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) { + for(Key key: starGraphs_ | boost::adaptors::map_keys) { // cout << "***** Node " << key << "*****" << endl; // initialize belief to the unary factor from the original graph DecisionTreeFactor::shared_ptr beliefAtKey; @@ -92,9 +92,9 @@ public: std::map messages; // eliminate each neighbor in this star graph one by one - BOOST_FOREACH(Key neighbor, starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { + for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { DiscreteFactorGraph subGraph; - BOOST_FOREACH(size_t factor, starGraphs_.at(key).varIndex_[neighbor]) { + for(size_t factor: starGraphs_.at(key).varIndex_[neighbor]) { subGraph.push_back(starGraphs_.at(key).star->at(factor)); } if (debug) subGraph.print("------- Subgraph:"); @@ -141,9 +141,9 @@ public: // Update corrected beliefs VariableIndex beliefFactors(*beliefs); - BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) { + for(Key key: starGraphs_ | boost::adaptors::map_keys) { std::map messages = allMessages[key]; - BOOST_FOREACH(Key neighbor, starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { + for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { DecisionTreeFactor correctedBelief = (*boost::dynamic_pointer_cast< DecisionTreeFactor>(beliefs->at(beliefFactors[key].front()))) / (*boost::dynamic_pointer_cast( @@ -169,13 +169,13 @@ private: const std::map& allDiscreteKeys) const { StarGraphs starGraphs; VariableIndex varIndex(graph); ///< access to all factors of each node - BOOST_FOREACH(Key key, varIndex | boost::adaptors::map_keys) { + for(Key key: varIndex | boost::adaptors::map_keys) { // initialize to multiply with other unary factors later DecisionTreeFactor::shared_ptr prodOfUnaries; // collect all factors involving this key in the original graph DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); - BOOST_FOREACH(size_t factorIdx, varIndex[key]) { + for(size_t factorIdx: varIndex[key]) { star->push_back(graph.at(factorIdx)); // accumulate unary factors @@ -196,7 +196,7 @@ private: KeySet neighbors = star->keys(); neighbors.erase(key); CorrectedBeliefIndices correctedBeliefIndices; - BOOST_FOREACH(Key neighbor, neighbors) { + for(Key neighbor: neighbors) { // TODO: default table for keys with more than 2 values? string initialBelief; for (size_t v = 0; v < allDiscreteKeys.at(neighbor).second - 1; ++v) { diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index fa68f04ea..0be4e4b1f 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -14,7 +14,6 @@ #include #include #include -#include using namespace boost::assign; using namespace std; diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index c1afe3882..c8c46ee7b 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other, const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); Matrix13 D_d_t1, D_d_t2; - double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + double d = distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; return d; diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index b44b14b63..fe1c83ce3 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -61,7 +61,7 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('K', 0), *noisy_K); noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); - graph.push_back(PriorFactor(Symbol('K', 0), *noisy_K, calNoise)); + graph.emplace_shared >(Symbol('K', 0), *noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -77,7 +77,7 @@ int main(int argc, char** argv){ } noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.push_back(PriorFactor(Symbol('x', pose_id), Pose3(m), poseNoise)); + graph.emplace_shared >(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys size_t x, l; @@ -89,9 +89,9 @@ int main(int argc, char** argv){ cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { -// graph.push_back( GenericStereoFactor(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K)); +// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); - graph.push_back(GeneralSFMFactor2(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0))); + graph.emplace_shared >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it @@ -107,7 +107,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; LevenbergMarquardtParams params; @@ -115,7 +115,7 @@ int main(int argc, char** argv){ params.verbosity = NonlinearOptimizerParams::ERROR; //create Levenberg-Marquardt optimizer to optimize the factor graph - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate,params); + LevenbergMarquardtOptimizer optimizer(graph, initial_estimate,params); // Values result = optimizer.optimize(); string K_values_file = "K_values.txt"; diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index f2fa1b31b..8df720cd1 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -309,19 +309,19 @@ int main(int argc, char** argv) { // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds cout << "After 15.0 seconds, each version contains to the following keys:" << endl; cout << " Concurrent Filter Keys: " << endl; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentFilter.getLinearizationPoint()) { + for(const Values::ConstKeyValuePair& key_value: concurrentFilter.getLinearizationPoint()) { cout << setprecision(5) << " Key: " << key_value.key << endl; } cout << " Concurrent Smoother Keys: " << endl; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentSmoother.getLinearizationPoint()) { + for(const Values::ConstKeyValuePair& key_value: concurrentSmoother.getLinearizationPoint()) { cout << setprecision(5) << " Key: " << key_value.key << endl; } cout << " Fixed-Lag Smoother Keys: " << endl; - BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, fixedlagSmoother.timestamps()) { + for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: fixedlagSmoother.timestamps()) { cout << setprecision(5) << " Key: " << key_timestamp.first << endl; } cout << " Batch Smoother Keys: " << endl; - BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, batchSmoother.timestamps()) { + for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: batchSmoother.timestamps()) { cout << setprecision(5) << " Key: " << key_timestamp.first << endl; } diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index ea1381bab..931e85c1a 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -133,11 +133,11 @@ int main(int argc, char** argv) { // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds cout << "After 3.0 seconds, " << endl; cout << " Batch Smoother Keys: " << endl; - BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, smootherBatch.timestamps()) { + for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherBatch.timestamps()) { cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; } cout << " iSAM2 Smoother Keys: " << endl; - BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, smootherISAM2.timestamps()) { + for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherISAM2.timestamps()) { cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; } diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index de1d3f77b..94a70470a 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(1,firstPose)); + graph.emplace_shared >(1,firstPose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -112,7 +112,7 @@ int main(int argc, char** argv){ cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); + LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params); Values result = optimizer.optimize(); cout << "Final result sample:" << endl; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 4062d0659..0ee601d26 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -43,7 +43,6 @@ #include // Standard headers, added last, so we know headers above work on their own -#include #include #include @@ -152,7 +151,7 @@ int main(int argc, char** argv) { typedef boost::shared_ptr SmartPtr; map smartFactors; if (smart) { - BOOST_FOREACH(size_t jj,ids) { + for(size_t jj: ids) { smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR)); newFactors.push_back(smartFactors[jj]); } @@ -166,7 +165,7 @@ int main(int argc, char** argv) { // Loop over odometry gttic_(iSAM); - BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) { + for(const TimedOdometry& timedOdometry: odometry) { //--------------------------------- odometry loop ----------------------------------------- double t; Pose2 odometry; @@ -219,25 +218,25 @@ int main(int argc, char** argv) { if (hasLandmarks) { // update landmark estimates landmarkEstimates = Values(); - BOOST_FOREACH(size_t jj,ids) + for(size_t jj: ids) landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj))); } newFactors = NonlinearFactorGraph(); initial = Values(); if (smart && !hasLandmarks) { cout << "initialize from smart landmarks" << endl; - BOOST_FOREACH(size_t jj,ids) { + for(size_t jj: ids) { Point2 landmark = smartFactors[jj]->triangulate(result); initial.insert(symbol('L', jj), landmark); landmarkEstimates.insert(symbol('L', jj), landmark); } } countK = 0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, result.filter()) + for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl; if (smart) { - BOOST_FOREACH(size_t jj,ids) { + for(size_t jj: ids) { Point2 landmark = smartFactors[jj]->triangulate(result); os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1" << endl; @@ -247,7 +246,7 @@ int main(int argc, char** argv) { i += 1; if (i>end) break; //--------------------------------- odometry loop ----------------------------------------- - } // BOOST_FOREACH + } // end for gttoc_(iSAM); // Print timings @@ -257,7 +256,7 @@ int main(int argc, char** argv) { Values result = isam.calculateEstimate(); ofstream os( "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt"); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, result.filter()) + for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; exit(0); diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 90d92897e..5f33a215b 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -42,7 +42,6 @@ #include // Standard headers, added last, so we know headers above work on their own -#include #include #include @@ -143,7 +142,7 @@ int main(int argc, char** argv) { // Loop over odometry gttic_(iSAM); - BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) { + for(const TimedOdometry& timedOdometry: odometry) { //--------------------------------- odometry loop ----------------------------------------- double t; Pose2 odometry; @@ -194,7 +193,7 @@ int main(int argc, char** argv) { } i += 1; //--------------------------------- odometry loop ----------------------------------------- - } // BOOST_FOREACH + } // end for gttoc_(iSAM); // Print timings @@ -204,12 +203,12 @@ int main(int argc, char** argv) { Values result = isam.calculateEstimate(); ofstream os2( "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt"); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, result.filter()) + for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl; ofstream os( "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt"); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, result.filter()) + for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; exit(0); diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index d3680460f..c8023b23c 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -126,7 +126,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -134,7 +134,7 @@ int main(int argc, char** argv){ cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); + LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params); Values result = optimizer.optimize(); cout << "Final result sample:" << endl; diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index dda2c32e6..c503514a6 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -18,7 +18,21 @@ */ #include +#include namespace gtsam { +/* ************************************************************************* */ +void Event::print(const std::string& s) const { + std::cout << s << "time = " << time_ << "location = " << location_.transpose(); +} + +/* ************************************************************************* */ +bool Event::equals(const Event& other, double tol) const { + return std::abs(time_ - other.time_) < tol + && traits::Equals(location_, other.location_, tol); +} + +/* ************************************************************************* */ + } //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 615b1d467..d9bacd106 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -21,6 +21,7 @@ #include #include +#include namespace gtsam { @@ -59,15 +60,10 @@ public: } /** print with optional string */ - void print(const std::string& s = "") const { - std::cout << s << "time = " << time_ << "location = " << location_.transpose(); - } + void print(const std::string& s = "") const; /** equals with an tolerance */ - bool equals(const Event& other, double tol = 1e-9) const { - return std::abs(time_ - other.time_) < tol - && traits::Equals(location_, other.location_, tol); - } + bool equals(const Event& other, double tol = 1e-9) const; /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { @@ -85,7 +81,7 @@ public: OptionalJacobian<1, 3> H2 = boost::none) const { static const double Speed = 330; Matrix13 D1, D2; - double distance = gtsam::distance(location_, microphone, D1, D2); + double distance = gtsam::distance3(location_, microphone, D1, D2); if (H1) // derivative of toa with respect to event *H1 << 1.0, D1 / Speed; diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 90e3eeea6..ba1445b20 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -4,7 +4,6 @@ */ #include -#include #include #include #include @@ -47,7 +46,7 @@ SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, doubl bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const { if (p.size() != size()) return false; for (size_t i=0; i::Equals(landmarks_[i], p.landmarks_[i], tol)) return false; return true; } @@ -55,8 +54,8 @@ bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const { /* ************************************************************************* */ void SimPolygon2D::print(const string& s) const { cout << "SimPolygon " << s << ": " << endl; - BOOST_FOREACH(const Point2& p, landmarks_) - p.print(" "); + for(const Point2& p: landmarks_) + traits::Print(p, " "); } /* ************************************************************************* */ @@ -73,7 +72,7 @@ bool SimPolygon2D::contains(const Point2& c) const { vector edges = walls(); bool initialized = false; bool lastSide = false; - BOOST_FOREACH(const SimWall2D& ab, edges) { + for(const SimWall2D& ab: edges) { // compute cross product of ab and ac Point2 dab = ab.b() - ab.a(); Point2 dac = c - ab.a(); @@ -97,10 +96,10 @@ bool SimPolygon2D::contains(const Point2& c) const { /* ************************************************************************* */ bool SimPolygon2D::overlaps(const SimPolygon2D& p) const { - BOOST_FOREACH(const Point2& a, landmarks_) + for(const Point2& a: landmarks_) if (p.contains(a)) return true; - BOOST_FOREACH(const Point2& a, p.landmarks_) + for(const Point2& a: p.landmarks_) if (contains(a)) return true; return false; @@ -108,7 +107,7 @@ bool SimPolygon2D::overlaps(const SimPolygon2D& p) const { /* ***************************************************************** */ bool SimPolygon2D::anyContains(const Point2& p, const vector& obstacles) { - BOOST_FOREACH(const SimPolygon2D& poly, obstacles) + for(const SimPolygon2D& poly: obstacles) if (poly.contains(p)) return true; return false; @@ -116,7 +115,7 @@ bool SimPolygon2D::anyContains(const Point2& p, const vector& obst /* ************************************************************************* */ bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector& obstacles) { - BOOST_FOREACH(const SimPolygon2D& poly, obstacles) + for(const SimPolygon2D& poly: obstacles) if (poly.overlaps(p)) return true; return false; @@ -134,7 +133,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( lms.push_back(Point2(-d2,-d2)); lms.push_back(Point2( d2,-d2)); - BOOST_FOREACH(const SimPolygon2D& poly, existing_polys) + for(const SimPolygon2D& poly: existing_polys) lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end()); for (size_t i=0; i min_side_len && + distance2(test_tri.landmark(1), test_tri.landmark(2)) > min_side_len && !nearExisting(lms, test_tri.landmark(0), min_vertex_dist) && !nearExisting(lms, test_tri.landmark(1), min_vertex_dist) && !nearExisting(lms, test_tri.landmark(2), min_vertex_dist) && @@ -188,7 +187,7 @@ SimPolygon2D SimPolygon2D::randomRectangle( lms.push_back(Point2(-d2, d2)); lms.push_back(Point2(-d2,-d2)); lms.push_back(Point2( d2,-d2)); - BOOST_FOREACH(const SimPolygon2D& poly, existing_polys) + for(const SimPolygon2D& poly: existing_polys) lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end()); const Point2 lower_corner(-side_len,-side_len); @@ -261,7 +260,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -273,7 +272,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -286,7 +285,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -304,7 +303,7 @@ Point2 SimPolygon2D::randomBoundedPoint2( return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -320,8 +319,8 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) { /* ***************************************************************** */ bool SimPolygon2D::nearExisting(const std::vector& S, const Point2& p, double threshold) { - BOOST_FOREACH(const Point2& Sp, S) - if (Sp.distance(p) < threshold) + for(const Point2& Sp: S) + if (distance2(Sp, p) < threshold) return true; return false; } diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index 9087cac2a..111d23b91 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -3,7 +3,6 @@ * @author Alex Cunningham */ -#include #include #include @@ -15,13 +14,14 @@ using namespace std; /* ************************************************************************* */ void SimWall2D::print(const std::string& s) const { std::cout << "SimWall2D " << s << ":" << std::endl; - a_.print(" a"); - b_.print(" b"); + traits::Print(a_, " a"); + traits::Print(b_, " b"); } /* ************************************************************************* */ bool SimWall2D::equals(const SimWall2D& other, double tol) const { - return a_.equals(other.a_, tol) && b_.equals(other.b_, tol); + return traits::Equals(a_, other.a_, tol) && + traits::Equals(b_, other.b_, tol); } /* ************************************************************************* */ @@ -38,8 +38,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons if (debug) cout << "len: " << len << endl; Point2 Ba = transform.transform_to(B.a()), Bb = transform.transform_to(B.b()); - if (debug) Ba.print("Ba"); - if (debug) Bb.print("Bb"); + if (debug) traits::Print(Ba, "Ba"); + if (debug) traits::Print(Bb, "Bb"); // check sides of line if (Ba.y() * Bb.y() > 0.0 || @@ -74,7 +74,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons } // find lower point by y - Point2 low, high; + Point2 low(0,0), high(0,0); if (Ba.y() > Bb.y()) { high = Ba; low = Bb; @@ -82,8 +82,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons high = Bb; low = Ba; } - if (debug) high.print("high"); - if (debug) low.print("low"); + if (debug) traits::Print(high, "high"); + if (debug) traits::Print(low, "low"); // find x-intercept double slope = (high.y() - low.y())/(high.x() - low.x()); @@ -135,11 +135,11 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, SimWall2D traj(test_pose.t(), cur_pose.t()); bool collision = false; Point2 intersection(1e+10, 1e+10); SimWall2D closest_wall; - BOOST_FOREACH(const SimWall2D& wall, walls) { + for(const SimWall2D& wall: walls) { Point2 cur_intersection; if (wall.intersects(traj,cur_intersection)) { collision = true; - if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) { + if (distance2(cur_pose.t(), cur_intersection) < distance2(cur_pose.t(), intersection)) { intersection = cur_intersection; closest_wall = wall; } @@ -155,7 +155,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, norm = norm / norm.norm(); // Simple check to make sure norm is on side closest robot - if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm)) + if (distance2(cur_pose.t(), intersection + norm) > distance2(cur_pose.t(), intersection - norm)) norm = - norm; // using the reflection diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index 38bba2ee3..c143bc36d 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -43,7 +43,7 @@ namespace gtsam { SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); } /** geometry */ - double length() const { return a_.distance(b_); } + double length() const { return distance2(a_, b_); } Point2 midpoint() const; /** diff --git a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp index 37cdfa0ba..6528f3f91 100644 --- a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp +++ b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp @@ -16,7 +16,7 @@ const double tol=1e-5; TEST(testPolygon, triangle_basic) { // create a triangle from points, extract landmarks/walls, check occupancy - Point2 pA, pB(2.0, 0.0), pC(0.0, 1.0); + Point2 pA(0,0), pB(2.0, 0.0), pC(0.0, 1.0); // construct and extract data SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC); diff --git a/gtsam_unstable/geometry/tests/testSimWall2D.cpp b/gtsam_unstable/geometry/tests/testSimWall2D.cpp index 62f458402..3bde734b2 100644 --- a/gtsam_unstable/geometry/tests/testSimWall2D.cpp +++ b/gtsam_unstable/geometry/tests/testSimWall2D.cpp @@ -24,7 +24,7 @@ TEST(testSimWall2D2D, construction ) { /* ************************************************************************* */ TEST(testSimWall2D2D, equals ) { - Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3; + Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3(0,0); SimWall2D w1(p1, p2), w2(p1, p3); EXPECT(assert_equal(w1, w1)); EXPECT(assert_inequal(w1, w2)); @@ -34,7 +34,7 @@ TEST(testSimWall2D2D, equals ) { /* ************************************************************************* */ TEST(testSimWall2D2D, intersection1 ) { SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0); - Point2 pt; + Point2 pt(0,0); EXPECT(w1.intersects(w2)); EXPECT(w2.intersects(w1)); w1.intersects(w2, pt); diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h new file mode 100644 index 000000000..18dc07aec --- /dev/null +++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h @@ -0,0 +1,290 @@ +/* ---------------------------------------------------------------------------- + + * 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 ActiveSetSolver-inl.h + * @brief Implmentation of ActiveSetSolver. + * @author Ivan Dario Jimenez + * @author Duy Nguyen Ta + * @date 2/11/16 + */ + +#include + +/******************************************************************************/ +// Convenient macros to reduce syntactic noise. undef later. +#define Template template +#define This ActiveSetSolver + +/******************************************************************************/ + +namespace gtsam { + +/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints + * If some inactive inequality constraints complain about the full step (alpha = 1), + * we have to adjust alpha to stay within the inequality constraints' feasible regions. + * + * For each inactive inequality j: + * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints + * - We want: aj'*(xk + alpha*p) - bj <= 0 + * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 + * it's good! + * - We only care when aj'*p > 0. In this case, we need to choose alpha so that + * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) + * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) + * + * We want the minimum of all those alphas among all inactive inequality. + */ +Template boost::tuple This::computeStepSize( + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p, const double& maxAlpha) const { + double minAlpha = maxAlpha; + int closestFactorIx = -1; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + double b = factor->getb()[0]; + // only check inactive factors + if (!factor->active()) { + // Compute a'*p + double aTp = factor->dotProductRow(p); + + // Check if a'*p >0. Don't care if it's not. + if (aTp <= 0) + continue; + + // Compute a'*xk + double aTx = factor->dotProductRow(xk); + + // alpha = (b - a'*xk) / (a'*p) + double alpha = (b - aTx) / aTp; + // We want the minimum of all those max alphas + if (alpha < minAlpha) { + closestFactorIx = factorIx; + minAlpha = alpha; + } + } + } + return boost::make_tuple(minAlpha, closestFactorIx); +} + +/******************************************************************************/ +/* + * The goal of this function is to find currently active inequality constraints + * that violate the condition to be active. The one that violates the condition + * the most will be removed from the active set. See Nocedal06book, pg 469-471 + * + * Find the BAD active inequality that pulls x strongest to the wrong direction + * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0) + * + * For active inequality constraints (those that are enforced as equality constraints + * in the current working set), we want lambda < 0. + * This is because: + * - From the Lagrangian L = f - lambda*c, we know that the constraint force + * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay + * on the constraint surface, the constraint force has to balance out with + * other unconstrained forces that are pulling x towards the unconstrained + * minimum point. The other unconstrained forces are pulling x toward (-\grad f), + * hence the constraint force has to be exactly \grad f, so that the total + * force is 0. + * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0), + * while we are solving for - (<=0) constraint. + * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction + * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied. + * That means we want lambda < 0. + * - This is because when the constrained force pulls x towards the infeasible region (+), + * the unconstrained force is pulling x towards the opposite direction into + * the feasible region (again because the total force has to be 0 to make x stay still) + * So we can drop this constraint to have a lower error but feasible solution. + * + * In short, active inequality constraints with lambda > 0 are BAD, because they + * violate the condition to be active. + * + * And we want to remove the worst one with the largest lambda from the active set. + * + */ +Template int This::identifyLeavingConstraint( + const InequalityFactorGraph& workingSet, + const VectorValues& lambdas) const { + int worstFactorIx = -1; + // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either + // inactive or a good inequality constraint, so we don't care! + double maxLambda = 0.0; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + if (factor->active()) { + double lambda = lambdas.at(factor->dualKey())[0]; + if (lambda > maxLambda) { + worstFactorIx = factorIx; + maxLambda = lambda; + } + } + } + return worstFactorIx; +} + +//****************************************************************************** +Template JacobianFactor::shared_ptr This::createDualFactor( + Key key, const InequalityFactorGraph& workingSet, + const VectorValues& delta) const { + // Transpose the A matrix of constrained factors to have the jacobian of the + // dual key + TermsContainer Aterms = collectDualJacobians( + key, problem_.equalities, equalityVariableIndex_); + TermsContainer AtermsInequalities = collectDualJacobians( + key, workingSet, inequalityVariableIndex_); + Aterms.insert(Aterms.end(), AtermsInequalities.begin(), + AtermsInequalities.end()); + + // Collect the gradients of unconstrained cost factors to the b vector + if (Aterms.size() > 0) { + Vector b = problem_.costGradient(key, delta); + // to compute the least-square approximation of dual variables + return boost::make_shared(Aterms, b); + } else { + return boost::make_shared(); + } +} + +/******************************************************************************/ +/* This function will create a dual graph that solves for the + * lagrange multipliers for the current working set. + * You can use lagrange multipliers as a necessary condition for optimality. + * The factor graph that is being solved is f' = -lambda * g' + * where f is the optimized function and g is the function resulting from + * aggregating the working set. + * The lambdas give you information about the feasibility of a constraint. + * if lambda < 0 the constraint is Ok + * if lambda = 0 you are on the constraint + * if lambda > 0 you are violating the constraint. + */ +Template GaussianFactorGraph::shared_ptr This::buildDualGraph( + const InequalityFactorGraph& workingSet, const VectorValues& delta) const { + GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + for (Key key : constrainedKeys_) { + // Each constrained key becomes a factor in the dual graph + JacobianFactor::shared_ptr dualFactor = + createDualFactor(key, workingSet, delta); + if (!dualFactor->empty()) dualGraph->push_back(dualFactor); + } + return dualGraph; +} + +//****************************************************************************** +Template GaussianFactorGraph +This::buildWorkingGraph(const InequalityFactorGraph& workingSet, + const VectorValues& xk) const { + GaussianFactorGraph workingGraph; + workingGraph.push_back(POLICY::buildCostFunction(problem_, xk)); + workingGraph.push_back(problem_.equalities); + for (const LinearInequality::shared_ptr& factor : workingSet) + if (factor->active()) workingGraph.push_back(factor); + return workingGraph; +} + +//****************************************************************************** +Template typename This::State This::iterate( + const typename This::State& state) const { + // Algorithm 16.3 from Nocedal06book. + // Solve with the current working set eqn 16.39, but instead of solving for p + // solve for x + GaussianFactorGraph workingGraph = + buildWorkingGraph(state.workingSet, state.values); + VectorValues newValues = workingGraph.optimize(); + // If we CAN'T move further + // if p_k = 0 is the original condition, modified by Duy to say that the state + // update is zero. + if (newValues.equals(state.values, 1e-7)) { + // Compute lambda from the dual graph + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, + newValues); + VectorValues duals = dualGraph->optimize(); + int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); + // If all inequality constraints are satisfied: We have the solution!! + if (leavingFactor < 0) { + return State(newValues, duals, state.workingSet, true, + state.iterations + 1); + } else { + // Inactivate the leaving constraint + InequalityFactorGraph newWorkingSet = state.workingSet; + newWorkingSet.at(leavingFactor)->inactivate(); + return State(newValues, duals, newWorkingSet, false, + state.iterations + 1); + } + } else { + // If we CAN make some progress, i.e. p_k != 0 + // Adapt stepsize if some inactive constraints complain about this move + double alpha; + int factorIx; + VectorValues p = newValues - state.values; + boost::tie(alpha, factorIx) = // using 16.41 + computeStepSize(state.workingSet, state.values, p, POLICY::maxAlpha); + // also add to the working set the one that complains the most + InequalityFactorGraph newWorkingSet = state.workingSet; + if (factorIx >= 0) + newWorkingSet.at(factorIx)->activate(); + // step! + newValues = state.values + alpha * p; + return State(newValues, state.duals, newWorkingSet, false, + state.iterations + 1); + } +} + +//****************************************************************************** +Template InequalityFactorGraph This::identifyActiveConstraints( + const InequalityFactorGraph& inequalities, + const VectorValues& initialValues, const VectorValues& duals, + bool useWarmStart) const { + InequalityFactorGraph workingSet; + for (const LinearInequality::shared_ptr& factor : inequalities) { + LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); + if (useWarmStart && duals.size() > 0) { + if (duals.exists(workingFactor->dualKey())) workingFactor->activate(); + else workingFactor->inactivate(); + } else { + double error = workingFactor->error(initialValues); + // Safety guard. This should not happen unless users provide a bad init + if (error > 0) throw InfeasibleInitialValues(); + if (fabs(error) < 1e-7) + workingFactor->activate(); + else + workingFactor->inactivate(); + } + workingSet.push_back(workingFactor); + } + return workingSet; +} + +//****************************************************************************** +Template std::pair This::optimize( + const VectorValues& initialValues, const VectorValues& duals, + bool useWarmStart) const { + // Initialize workingSet from the feasible initialValues + InequalityFactorGraph workingSet = identifyActiveConstraints( + problem_.inequalities, initialValues, duals, useWarmStart); + State state(initialValues, duals, workingSet, false, 0); + + /// main loop of the solver + while (!state.converged) state = iterate(state); + + return std::make_pair(state.values, state.duals); +} + +//****************************************************************************** +Template std::pair This::optimize() const { + INITSOLVER initSolver(problem_); + VectorValues initValues = initSolver.solve(); + return optimize(initValues); +} + +} + +#undef Template +#undef This \ No newline at end of file diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h new file mode 100644 index 000000000..a5c60f311 --- /dev/null +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -0,0 +1,204 @@ +/* ---------------------------------------------------------------------------- + + * 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 ActiveSetSolver.h + * @brief Active set method for solving LP, QP problems + * @author Ivan Dario Jimenez + * @author Duy Nguyen Ta + * @date 1/25/16 + */ +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * This class implements the active set algorithm for solving convex + * Programming problems. + * + * @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or + * QP (quadratic program). + * @tparam POLICY specific detail policy tailored for the particular program + * @tparam INITSOLVER Solver for an initial feasible solution of this problem. + */ +template +class ActiveSetSolver { +public: + /// This struct contains the state information for a single iteration + struct State { + VectorValues values; //!< current best values at each step + VectorValues duals; //!< current values of dual variables at each step + InequalityFactorGraph workingSet; /*!< keep track of current active/inactive + inequality constraints */ + bool converged; //!< True if the algorithm has converged to a solution + size_t iterations; /*!< Number of iterations. Incremented at the end of + each iteration. */ + + /// Default constructor + State() + : values(), duals(), workingSet(), converged(false), iterations(0) {} + + /// Constructor with initial values + State(const VectorValues& initialValues, const VectorValues& initialDuals, + const InequalityFactorGraph& initialWorkingSet, bool _converged, + size_t _iterations) + : values(initialValues), + duals(initialDuals), + workingSet(initialWorkingSet), + converged(_converged), + iterations(_iterations) {} + }; + +protected: + const PROBLEM& problem_; //!< the particular [convex] problem to solve + VariableIndex equalityVariableIndex_, + inequalityVariableIndex_; /*!< index to corresponding factors to build + dual graphs */ + KeySet constrainedKeys_; /*!< all constrained keys, will become factors in + dual graphs */ + + /// Vector of key matrix pairs. Matrices are usually the A term for a factor. + typedef std::vector > TermsContainer; + +public: + /// Constructor + ActiveSetSolver(const PROBLEM& problem) : problem_(problem) { + equalityVariableIndex_ = VariableIndex(problem_.equalities); + inequalityVariableIndex_ = VariableIndex(problem_.inequalities); + constrainedKeys_ = problem_.equalities.keys(); + constrainedKeys_.merge(problem_.inequalities.keys()); + } + + /** + * Optimize with provided initial values + * For this version, it is the responsibility of the caller to provide + * a feasible initial value, otherwise, an exception will be thrown. + * @return a pair of solutions + */ + std::pair optimize( + const VectorValues& initialValues, + const VectorValues& duals = VectorValues(), + bool useWarmStart = false) const; + + /** + * For this version the caller will not have to provide an initial value + * @return a pair of solutions + */ + std::pair optimize() const; + +protected: + /** + * Compute minimum step size alpha to move from the current point @p xk to the + * next feasible point along a direction @p p: x' = xk + alpha*p, + * where alpha \in [0,maxAlpha]. + * + * For QP, maxAlpha = 1. For LP: maxAlpha = Inf. + * + * @return a tuple of (minAlpha, closestFactorIndex) where closestFactorIndex + * is the closest inactive inequality constraint that blocks xk to move + * further and that has the minimum alpha, or (-1, maxAlpha) if there is no + * such inactive blocking constraint. + * + * If there is a blocking constraint, the closest one will be added to the + * working set and become active in the next iteration. + */ + boost::tuple computeStepSize( + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p, const double& maxAlpha) const; + + /** + * Finds the active constraints in the given factor graph and returns the + * Dual Jacobians used to build a dual factor graph. + */ + template + TermsContainer collectDualJacobians(Key key, const FactorGraph& graph, + const VariableIndex& variableIndex) const { + /* + * Iterates through each factor in the factor graph and checks + * whether it's active. If the factor is active it reutrns the A + * term of the factor. + */ + TermsContainer Aterms; + if (variableIndex.find(key) != variableIndex.end()) { + for (size_t factorIx : variableIndex[key]) { + typename FACTOR::shared_ptr factor = graph.at(factorIx); + if (!factor->active()) + continue; + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } + } + return Aterms; + } + + /** + * Creates a dual factor from the current workingSet and the key of the + * the variable used to created the dual factor. + */ + JacobianFactor::shared_ptr createDualFactor( + Key key, const InequalityFactorGraph& workingSet, + const VectorValues& delta) const; + +public: /// Just for testing... + + /// Builds a dual graph from the current working set. + GaussianFactorGraph::shared_ptr buildDualGraph( + const InequalityFactorGraph& workingSet, const VectorValues& delta) const; + + /** + * Build a working graph of cost, equality and active inequality constraints + * to solve at each iteration. + * @param workingSet the collection of all cost and constrained factors + * @param xk current solution, used to build a special quadratic cost in LP + * @return a new better solution + */ + GaussianFactorGraph buildWorkingGraph( + const InequalityFactorGraph& workingSet, + const VectorValues& xk = VectorValues()) const; + + /// Iterate 1 step, return a new state with a new workingSet and values + State iterate(const State& state) const; + + /// Identify active constraints based on initial values. + InequalityFactorGraph identifyActiveConstraints( + const InequalityFactorGraph& inequalities, + const VectorValues& initialValues, + const VectorValues& duals = VectorValues(), + bool useWarmStart = false) const; + + /// Identifies active constraints that shouldn't be active anymore. + int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, + const VectorValues& lambdas) const; + +}; + +/** + * Find the max key in a problem. + * Useful to determine unique keys for additional slack variables + */ +template +Key maxKey(const PROBLEM& problem) { + auto keys = problem.cost.keys(); + Key maxKey = *std::max_element(keys.begin(), keys.end()); + if (!problem.equalities.empty()) + maxKey = std::max(maxKey, *problem.equalities.keys().rbegin()); + if (!problem.inequalities.empty()) + maxKey = std::max(maxKey, *problem.inequalities.keys().rbegin()); + return maxKey; +} + +} // namespace gtsam + +#include \ No newline at end of file diff --git a/gtsam_unstable/linear/EqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h new file mode 100644 index 000000000..43befdbe0 --- /dev/null +++ b/gtsam_unstable/linear/EqualityFactorGraph.h @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 EqualityFactorGraph.h + * @brief Factor graph of all LinearEquality factors + * @date Dec 8, 2014 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Collection of all Linear Equality constraints Ax=b of + * a Programming problem as a Factor Graph + */ +class EqualityFactorGraph: public FactorGraph { +public: + typedef boost::shared_ptr shared_ptr; + + /// Compute error of a guess. + double error(const VectorValues& x) const { + double total_error = 0.; + for (const sharedFactor& factor : *this) { + if (factor) + total_error += factor->error(x); + } + return total_error; + } +}; + +/// traits +template<> struct traits : public Testable< + EqualityFactorGraph> { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h new file mode 100644 index 000000000..c87645697 --- /dev/null +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * 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 InequalityFactorGraph.h + * @brief Factor graph of all LinearInequality factors + * @date Dec 8, 2014 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Collection of all Linear Inequality constraints Ax-b <= 0 of + * a Programming problem as a Factor Graph + */ +class InequalityFactorGraph: public FactorGraph { +private: + typedef FactorGraph Base; + +public: + typedef boost::shared_ptr shared_ptr; + + /** print */ + void print(const std::string& str, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + Base::print(str, keyFormatter); + } + + /** equals */ + bool equals(const InequalityFactorGraph& other, double tol = 1e-9) const { + return Base::equals(other, tol); + } + + /** + * Compute error of a guess. + * Infinity error if it violates an inequality; zero otherwise. */ + double error(const VectorValues& x) const { + for (const sharedFactor& factor : *this) { + if (factor) + if (factor->error(x) > 1e-7) + return std::numeric_limits::infinity(); + } + return 0.0; + } +}; + +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/InfeasibleInitialValues.h b/gtsam_unstable/linear/InfeasibleInitialValues.h new file mode 100644 index 000000000..60adb872e --- /dev/null +++ b/gtsam_unstable/linear/InfeasibleInitialValues.h @@ -0,0 +1,45 @@ +/* ---------------------------------------------------------------------------- + + * 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 InfeasibleInitialValues.h + * @brief Exception thrown when given Infeasible Initial Values. + * @date jan 24, 2015 + * @author Duy-Nguyen Ta + */ + +#pragma once + +namespace gtsam { +/* ************************************************************************* */ +/** An exception indicating that the provided initial value is infeasible + * Also used to inzdicatethat the noise model dimension passed into a + * JacobianFactor has a different dimensionality than the factor. */ +class InfeasibleInitialValues: public ThreadsafeException< + InfeasibleInitialValues> { +public: + InfeasibleInitialValues() { + } + + virtual ~InfeasibleInitialValues() throw () { + } + + virtual const char *what() const throw () { + if (description_.empty()) + description_ = + "An infeasible initial value was provided for the solver.\n"; + return description_.c_str(); + } + +private: + mutable std::string description_; +}; +} diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h new file mode 100644 index 000000000..f7c5a5c79 --- /dev/null +++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h @@ -0,0 +1,40 @@ +/* ---------------------------------------------------------------------------- + + * 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 InfeasibleOrUnboundedProblem.h + * @brief Throw when the problem is either infeasible or unbounded + * @author Ivan Dario Jimenez + * @date 1/24/16 + */ + +#pragma once + +namespace gtsam { + +class InfeasibleOrUnboundedProblem: public ThreadsafeException< + InfeasibleOrUnboundedProblem> { +public: + InfeasibleOrUnboundedProblem() { + } + virtual ~InfeasibleOrUnboundedProblem() throw () { + } + + virtual const char* what() const throw () { + if (description_.empty()) + description_ = "The problem is either infeasible or unbounded.\n"; + return description_.c_str(); + } + +private: + mutable std::string description_; +}; +} diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h new file mode 100644 index 000000000..fc00c2240 --- /dev/null +++ b/gtsam_unstable/linear/LP.h @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 LP.h + * @brief Struct used to hold a Linear Programming Problem + * @author Ivan Dario Jimenez + * @date 1/24/16 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +using namespace std; + +/// Mapping between variable's key and its corresponding dimensionality +using KeyDimMap = std::map; +/* + * Iterates through every factor in a linear graph and generates a + * mapping between every factor key and it's corresponding dimensionality. + */ +template +KeyDimMap collectKeyDim(const LinearGraph& linearGraph) { + KeyDimMap keyDimMap; + for (const typename LinearGraph::sharedFactor& factor : linearGraph) { + if (!factor) continue; + for (Key key : factor->keys()) + keyDimMap[key] = factor->getDim(factor->find(key)); + } + return keyDimMap; +} + +/** + * Data structure of a Linear Program + */ +struct LP { + using shared_ptr = boost::shared_ptr; + + LinearCost cost; //!< Linear cost factor + EqualityFactorGraph equalities; //!< Linear equality constraints: cE(x) = 0 + InequalityFactorGraph inequalities; //!< Linear inequality constraints: cI(x) <= 0 +private: + mutable KeyDimMap cachedConstrainedKeyDimMap_; //!< cached key-dim map of all variables in the constraints + +public: + /// check feasibility + bool isFeasible(const VectorValues& x) const { + return (equalities.error(x) == 0 && inequalities.error(x) == 0); + } + + /// print + void print(const string& s = "") const { + std::cout << s << std::endl; + cost.print("Linear cost: "); + equalities.print("Linear equality factors: "); + inequalities.print("Linear inequality factors: "); + } + + /// equals + bool equals(const LP& other, double tol = 1e-9) const { + return cost.equals(other.cost) && equalities.equals(other.equalities) + && inequalities.equals(other.inequalities); + } + + const KeyDimMap& constrainedKeyDimMap() const { + if (!cachedConstrainedKeyDimMap_.empty()) + return cachedConstrainedKeyDimMap_; + // Collect key-dim map of all variables in the constraints + cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); + KeyDimMap keysDim2 = collectKeyDim(inequalities); + cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); + return cachedConstrainedKeyDimMap_; + } + + Vector costGradient(Key key, const VectorValues& delta) const { + Vector g = Vector::Zero(delta.at(key).size()); + Factor::const_iterator it = cost.find(key); + if (it != cost.end()) g = cost.getA(it).transpose(); + return g; + } +}; + +/// traits +template<> struct traits : public Testable { +}; + +} diff --git a/gtsam_unstable/linear/LPInitSolver.cpp b/gtsam_unstable/linear/LPInitSolver.cpp new file mode 100644 index 000000000..8c3df3132 --- /dev/null +++ b/gtsam_unstable/linear/LPInitSolver.cpp @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * 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 LPInitSolver.h + * @brief This finds a feasible solution for an LP problem + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 6/16/16 + */ + +#include +#include +#include + +namespace gtsam { + +/******************************************************************************/ +VectorValues LPInitSolver::solve() const { + // Build the graph to solve for the initial value of the initial problem + GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph(); + VectorValues x0 = initOfInitGraph->optimize(); + double y0 = compute_y0(x0); + Key yKey = maxKey(lp_) + 1; // the unique key for y0 + VectorValues xy0(x0); + xy0.insert(yKey, Vector::Constant(1, y0)); + + // Formulate and solve the initial LP + LP::shared_ptr initLP = buildInitialLP(yKey); + + // solve the initialLP + LPSolver lpSolveInit(*initLP); + VectorValues xyInit = lpSolveInit.optimize(xy0).first; + double yOpt = xyInit.at(yKey)[0]; + xyInit.erase(yKey); + if (yOpt > 0) + throw InfeasibleOrUnboundedProblem(); + else + return xyInit; +} + +/******************************************************************************/ +LP::shared_ptr LPInitSolver::buildInitialLP(Key yKey) const { + LP::shared_ptr initLP(new LP()); + initLP->cost = LinearCost(yKey, I_1x1); // min y + initLP->equalities = lp_.equalities; // st. Ax = b + initLP->inequalities = + addSlackVariableToInequalities(yKey, + lp_.inequalities); // Cx-y <= d + return initLP; +} + +/******************************************************************************/ +GaussianFactorGraph::shared_ptr LPInitSolver::buildInitOfInitGraph() const { + // first add equality constraints Ax = b + GaussianFactorGraph::shared_ptr initGraph( + new GaussianFactorGraph(lp_.equalities)); + + // create factor ||x||^2 and add to the graph + const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap(); + for (Key key : constrainedKeyDim | boost::adaptors::map_keys) { + size_t dim = constrainedKeyDim.at(key); + initGraph->push_back( + JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim))); + } + return initGraph; +} + +/******************************************************************************/ +double LPInitSolver::compute_y0(const VectorValues& x0) const { + double y0 = -std::numeric_limits::infinity(); + for (const auto& factor : lp_.inequalities) { + double error = factor->error(x0); + if (error > y0) y0 = error; + } + return y0; +} + +/******************************************************************************/ +std::vector > LPInitSolver::collectTerms( + const LinearInequality& factor) const { + std::vector > terms; + for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) { + terms.push_back(make_pair(*it, factor.getA(it))); + } + return terms; +} + +/******************************************************************************/ +InequalityFactorGraph LPInitSolver::addSlackVariableToInequalities( + Key yKey, const InequalityFactorGraph& inequalities) const { + InequalityFactorGraph slackInequalities; + for (const auto& factor : lp_.inequalities) { + std::vector > terms = collectTerms(*factor); // Cx + terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y + double d = factor->getb()[0]; + slackInequalities.push_back(LinearInequality(terms, d, factor->dualKey())); + } + return slackInequalities; +} + +} \ No newline at end of file diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h new file mode 100644 index 000000000..4eb672fbc --- /dev/null +++ b/gtsam_unstable/linear/LPInitSolver.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 LPInitSolver.h + * @brief This LPInitSolver implements the strategy in Matlab. + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 1/24/16 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { +/** + * This LPInitSolver implements the strategy in Matlab: + * http://www.mathworks.com/help/optim/ug/linear-programming-algorithms.html#brozyzb-9 + * Solve for x and y: + * min y + * st Ax = b + * Cx - y <= d + * where y \in R, x \in R^n, and Ax = b and Cx <= d is the constraints of the original problem. + * + * If the solution for this problem {x*,y*} has y* <= 0, we'll have x* a feasible initial point + * of the original problem + * otherwise, if y* > 0 or the problem has no solution, the original problem is infeasible. + * + * The initial value of this initial problem can be found by solving + * min ||x||^2 + * s.t. Ax = b + * to have a solution x0 + * then y = max_j ( Cj*x0 - dj ) -- due to the constraints y >= Cx - d + * + * WARNING: If some xj in the inequality constraints does not exist in the equality constraints, + * set them as zero for now. If that is the case, the original problem doesn't have a unique + * solution (it could be either infeasible or unbounded). + * So, if the initialization fails because we enforce xj=0 in the problematic + * inequality constraint, we can't conclude that the problem is infeasible. + * However, whether it is infeasible or unbounded, we don't have a unique solution anyway. + */ +class LPInitSolver { +private: + const LP& lp_; + +public: + /// Construct with an LP problem + LPInitSolver(const LP& lp) : lp_(lp) {} + + ///@return a feasible initialization point + VectorValues solve() const; + +private: + /// build initial LP + LP::shared_ptr buildInitialLP(Key yKey) const; + + /** + * Build the following graph to solve for an initial value of the initial problem + * min ||x||^2 s.t. Ax = b + */ + GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const; + + /// y = max_j ( Cj*x0 - dj ) -- due to the inequality constraints y >= Cx - d + double compute_y0(const VectorValues& x0) const; + + /// Collect all terms of a factor into a container. + std::vector> collectTerms( + const LinearInequality& factor) const; + + /// Turn Cx <= d into Cx - y <= d factors + InequalityFactorGraph addSlackVariableToInequalities(Key yKey, + const InequalityFactorGraph& inequalities) const; + + // friend class for unit-testing private methods + FRIEND_TEST(LPInitSolver, initialization); +}; + +} diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp new file mode 100644 index 000000000..c1319a5ec --- /dev/null +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -0,0 +1,25 @@ +/* ---------------------------------------------------------------------------- + + * 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 LPSolver.cpp + * @brief + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 1/26/16 + */ + +#include + +namespace gtsam { +constexpr double LPPolicy::maxAlpha; +} + diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h new file mode 100644 index 000000000..153aa7fda --- /dev/null +++ b/gtsam_unstable/linear/LPSolver.h @@ -0,0 +1,79 @@ +/* ---------------------------------------------------------------------------- + + * 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 LPSolver.h + * @brief Policy of ActiveSetSolver to solve Linear Programming Problems + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 6/16/16 + */ + +#include +#include +#include + +#include +#include + +namespace gtsam { + +/// Policy for ActivetSetSolver to solve Linear Programming \sa LP problems +struct LPPolicy { + /// Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient + /// For LP, maxAlpha = Infinity + static constexpr double maxAlpha = std::numeric_limits::infinity(); + + /** + * Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution + * on the constraint surface and g is the gradient of the linear cost, + * i.e. -g is the direction we wish to follow to decrease the cost. + * + * Essentially, we try to match the direction d = x-xk with -g as much as possible + * subject to the condition that x needs to be on the constraint surface, i.e., d is + * along the surface's subspace. + * + * The least-square solution of this quadratic subject to a set of linear constraints + * is the projection of the gradient onto the constraints' subspace + */ + static GaussianFactorGraph buildCostFunction(const LP& lp, + const VectorValues& xk) { + GaussianFactorGraph graph; + for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end(); + ++it) { + size_t dim = lp.cost.getDim(it); + Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g + graph.emplace_shared(*it, Matrix::Identity(dim, dim), b); + } + + KeySet allKeys = lp.inequalities.keys(); + allKeys.merge(lp.equalities.keys()); + allKeys.merge(KeySet(lp.cost.keys())); + // Add corresponding factors for all variables that are not explicitly in + // the cost function. Gradients of the cost function wrt to these variables + // are zero (g=0), so b=xk + if (lp.cost.keys().size() != allKeys.size()) { + KeySet difference; + std::set_difference(allKeys.begin(), allKeys.end(), lp.cost.begin(), + lp.cost.end(), + std::inserter(difference, difference.end())); + for (Key k : difference) { + size_t dim = lp.constrainedKeyDimMap().at(k); + graph.emplace_shared(k, Matrix::Identity(dim, dim), xk.at(k)); + } + } + return graph; + } +}; + +using LPSolver = ActiveSetSolver; + +} diff --git a/gtsam_unstable/linear/LinearCost.h b/gtsam_unstable/linear/LinearCost.h new file mode 100644 index 000000000..b489510af --- /dev/null +++ b/gtsam_unstable/linear/LinearCost.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 LinearCost.h + * @brief LinearCost derived from JacobianFactor to support linear cost functions c'x + * @date Nov 27, 2014 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include + +namespace gtsam { + +typedef Eigen::RowVectorXd RowVector; + +/** + * This class defines a linear cost function c'x + * which is a JacobianFactor with only one row + */ +class LinearCost: public JacobianFactor { +public: + typedef LinearCost This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +public: + /** default constructor for I/O */ + LinearCost() : + Base() { + } + + /** Conversion from HessianFactor */ + explicit LinearCost(const HessianFactor& hf) { + throw std::runtime_error("Cannot convert HessianFactor to LinearCost"); + } + + /** Conversion from JacobianFactor */ + explicit LinearCost(const JacobianFactor& jf) : + Base(jf) { + if (jf.isConstrained()) { + throw std::runtime_error( + "Cannot convert a constrained JacobianFactor to LinearCost"); + } + + if (jf.get_model()->dim() != 1) { + throw std::runtime_error( + "Only support single-valued linear cost factor!"); + } + } + + /** Construct unary factor */ + LinearCost(Key i1, const RowVector& A1) : + Base(i1, A1, Vector1::Zero()) { + } + + /** Construct binary factor */ + LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) : + Base(i1, A1, i2, A2, Vector1::Zero()) { + } + + /** Construct ternary factor */ + LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, + const RowVector& A3) : + Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearCost(const TERMS& terms) : + Base(terms, Vector1::Zero()) { + } + + /** Virtual destructor */ + virtual ~LinearCost() { + } + + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + Base::print(s + " LinearCost: ", formatter); + } + + /** Clone this LinearCost */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearCost > (*this)); + } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for single-valued inequality constraints. */ + virtual double error(const VectorValues& c) const { + return error_vector(c)[0]; + } +}; +// \ LinearCost + +/// traits +template<> struct traits : public Testable { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h index bc1b2bc12..2463ef31c 100644 --- a/gtsam_unstable/linear/LinearEquality.h +++ b/gtsam_unstable/linear/LinearEquality.h @@ -9,11 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * LinearEquality.h - * @brief: LinearEquality derived from Base with constrained noise model - * @date: Nov 27, 2014 - * @author: thduynguyen +/** + * @file LinearEquality.h + * @brief LinearEquality derived from Base with constrained noise model + * @date Nov 27, 2014 + * @author Duy-Nguyen Ta */ #pragma once @@ -23,7 +23,7 @@ namespace gtsam { /** - * This class defines Linear constraints by inherit Base + * This class defines a linear equality constraints, inheriting JacobianFactor * with the special Constrained noise model */ class LinearEquality: public JacobianFactor { @@ -41,6 +41,17 @@ public: Base() { } + /** + * Construct from a constrained noisemodel JacobianFactor with a dual key. + */ + explicit LinearEquality(const JacobianFactor& jf, Key dualKey) : + Base(jf), dualKey_(dualKey) { + if (!jf.isConstrained()) { + throw std::runtime_error( + "Cannot convert an unconstrained JacobianFactor to LinearEquality"); + } + } + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ explicit LinearEquality(const HessianFactor& hf) { throw std::runtime_error("Cannot convert HessianFactor to LinearEquality"); @@ -90,15 +101,19 @@ public: /** Clone this LinearEquality */ virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearEquality > (*this)); } /// dual key - Key dualKey() const { return dualKey_; } + Key dualKey() const { + return dualKey_; + } /// for active set method: equality constraints are always active - bool active() const { return true; } + bool active() const { + return true; + } /** Special error_vector for constraints (A*x-b) */ Vector error_vector(const VectorValues& c) const { @@ -113,11 +128,12 @@ public: return 0.0; } -}; // \ LinearEquality - +}; +// \ LinearEquality /// traits -template<> struct traits : public Testable {}; +template<> struct traits : public Testable { +}; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/LinearEqualityFactorGraph.h deleted file mode 100644 index 9c067ae3d..000000000 --- a/gtsam_unstable/linear/LinearEqualityFactorGraph.h +++ /dev/null @@ -1,37 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/* - * LinearEqualityFactorGraph.h - * @brief: Factor graph of all LinearEquality factors - * @date: Dec 8, 2014 - * @author: thduynguyen - */ - -#pragma once - -#include -#include - -namespace gtsam { - -class LinearEqualityFactorGraph : public FactorGraph { -public: - typedef boost::shared_ptr shared_ptr; -}; - -/// traits -template<> struct traits : public Testable< - LinearEqualityFactorGraph> { -}; - -} // \ namespace gtsam - diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index 7c62c3d54..1a31bd4e4 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -9,24 +9,26 @@ * -------------------------------------------------------------------------- */ -/* - * LinearInequality.h - * @brief: LinearInequality derived from Base with constrained noise model - * @date: Nov 27, 2014 - * @author: thduynguyen +/** + * @file LinearInequality.h + * @brief LinearInequality derived from Base with constrained noise model + * @date Nov 27, 2014 + * @author Duy-Nguyen Ta + * @author Ivan Dario Jimenez */ #pragma once #include +#include namespace gtsam { typedef Eigen::RowVectorXd RowVector; /** - * This class defines Linear constraints by inherit Base - * with the special Constrained noise model + * This class defines a linear inequality constraint Ax-b <= 0, + * inheriting JacobianFactor with the special Constrained noise model */ class LinearInequality: public JacobianFactor { public: @@ -44,35 +46,49 @@ public: Base(), active_(true) { } - /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + /** Conversion from HessianFactor */ explicit LinearInequality(const HessianFactor& hf) { throw std::runtime_error( "Cannot convert HessianFactor to LinearInequality"); } + /** Conversion from JacobianFactor */ + explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : + Base(jf), dualKey_(dualKey), active_(true) { + if (!jf.isConstrained()) { + throw std::runtime_error( + "Cannot convert an unconstrained JacobianFactor to LinearInequality"); + } + + if (jf.get_model()->dim() != 1) { + throw std::runtime_error("Only support single-valued inequality factor!"); + } + } + /** Construct unary factor */ LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) : - Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( - dualKey), active_(true) { + Base(i1, A1, (Vector(1) << b).finished(), + noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } /** Construct binary factor */ - LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b, - Key dualKey) : - Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( - dualKey), active_(true) { + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, + double b, Key dualKey) : + Base(i1, A1, i2, A2, (Vector(1) << b).finished(), + noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } /** Construct ternary factor */ - LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, - const RowVector& A3, double b, Key dualKey) : + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, + Key i3, const RowVector& A3, double b, Key dualKey) : Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ + * collection of keys and matrices making up the factor. + * In this inequality factor, each matrix must have only one row!! */ template LinearInequality(const TERMS& terms, double b, Key dualKey) : Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( @@ -99,21 +115,29 @@ public: /** Clone this LinearInequality */ virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearInequality > (*this)); } /// dual key - Key dualKey() const { return dualKey_; } + Key dualKey() const { + return dualKey_; + } /// return true if this constraint is active - bool active() const { return active_; } + bool active() const { + return active_; + } /// Make this inequality constraint active - void activate() { active_ = true; } + void activate() { + active_ = true; + } /// Make this inequality constraint inactive - void inactivate() { active_ = false; } + void inactivate() { + active_ = false; + } /** Special error_vector for constraints (A*x-b) */ Vector error_vector(const VectorValues& c) const { @@ -136,10 +160,12 @@ public: return aTp; } -}; // \ LinearInequality +}; +// \ LinearInequality /// traits -template<> struct traits : public Testable {}; +template<> struct traits : public Testable { +}; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h deleted file mode 100644 index eca271941..000000000 --- a/gtsam_unstable/linear/LinearInequalityFactorGraph.h +++ /dev/null @@ -1,52 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/* - * LinearInequalityFactorGraph.h - * @brief: Factor graph of all LinearInequality factors - * @date: Dec 8, 2014 - * @author: thduynguyen - */ - -#pragma once - -#include -#include - -namespace gtsam { - -class LinearInequalityFactorGraph: public FactorGraph { -private: - typedef FactorGraph Base; - -public: - typedef boost::shared_ptr shared_ptr; - - /** print */ - void print(const std::string& str, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - Base::print(str, keyFormatter); - } - - /** equals */ - bool equals(const LinearInequalityFactorGraph& other, - double tol = 1e-9) const { - return Base::equals(other, tol); - } -}; - -/// traits -template<> struct traits : public Testable< - LinearInequalityFactorGraph> { -}; - -} // \ namespace gtsam - diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h index 111ab506f..e610eb934 100644 --- a/gtsam_unstable/linear/QP.h +++ b/gtsam_unstable/linear/QP.h @@ -9,29 +9,34 @@ * -------------------------------------------------------------------------- */ -/* - * QP.h - * @brief: Factor graphs of a Quadratic Programming problem - * @date: Dec 8, 2014 - * @author: thduynguyen +/** + * @file QP.h + * @brief Factor graphs of a Quadratic Programming problem + * @date Dec 8, 2014 + * @author Duy-Nguyen Ta */ #pragma once #include -#include -#include +#include +#include +#include namespace gtsam { /** - * struct contains factor graphs of a Quadratic Programming problem + * Struct contains factor graphs of a Quadratic Programming problem */ struct QP { GaussianFactorGraph cost; //!< Quadratic cost factors - LinearEqualityFactorGraph equalities; //!< linear equality constraints - LinearInequalityFactorGraph inequalities; //!< linear inequality constraints + EqualityFactorGraph equalities; //!< linear equality constraints: cE(x) = 0 + InequalityFactorGraph inequalities; //!< linear inequality constraints: cI(x) <= 0 +private: + mutable VariableIndex cachedCostVariableIndex_; + +public: /** default constructor */ QP() : cost(), equalities(), inequalities() { @@ -39,8 +44,8 @@ struct QP { /** constructor */ QP(const GaussianFactorGraph& _cost, - const LinearEqualityFactorGraph& _linearEqualities, - const LinearInequalityFactorGraph& _linearInequalities) : + const EqualityFactorGraph& _linearEqualities, + const InequalityFactorGraph& _linearInequalities) : cost(_cost), equalities(_linearEqualities), inequalities( _linearInequalities) { } @@ -52,6 +57,23 @@ struct QP { equalities.print("Linear equality factors: "); inequalities.print("Linear inequality factors: "); } + + const VariableIndex& costVariableIndex() const { + if (cachedCostVariableIndex_.size() == 0) + cachedCostVariableIndex_ = VariableIndex(cost); + return cachedCostVariableIndex_; + } + + Vector costGradient(Key key, const VectorValues& delta) const { + Vector g = Vector::Zero(delta.at(key).size()); + if (costVariableIndex().find(key) != costVariableIndex().end()) { + for (size_t factorIx : costVariableIndex()[key]) { + GaussianFactor::shared_ptr factor = cost.at(factorIx); + g += factor->gradient(key, delta); + } + } + return g; + } }; } // namespace gtsam diff --git a/gtsam_unstable/linear/QPInitSolver.h b/gtsam_unstable/linear/QPInitSolver.h new file mode 100644 index 000000000..6742e9223 --- /dev/null +++ b/gtsam_unstable/linear/QPInitSolver.h @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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 QPInitSolver.h + * @brief This finds a feasible solution for a QP problem + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 6/16/16 + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * This class finds a feasible solution for a QP problem. + * This uses the Matlab strategy for initialization + * For details, see + * http://www.mathworks.com/help/optim/ug/quadratic-programming-algorithms.html#brrzwpf-22 + */ +class QPInitSolver { + const QP& qp_; +public: + /// Constructor with a QP problem + QPInitSolver(const QP& qp) : qp_(qp) {} + + ///@return a feasible initialization point + VectorValues solve() const { + // Make an LP with any linear cost function. It doesn't matter for + // initialization. + LP initProblem; + // make an unrelated key for a random variable cost + Key newKey = maxKey(qp_) + 1; + initProblem.cost = LinearCost(newKey, Vector::Ones(1)); + initProblem.equalities = qp_.equalities; + initProblem.inequalities = qp_.inequalities; + LPInitSolver initSolver(initProblem); + return initSolver.solve(); + } +}; + + +} \ No newline at end of file diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp new file mode 100644 index 000000000..a6fc072c7 --- /dev/null +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 QPParser.cpp + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#define BOOST_SPIRIT_USE_PHOENIX_V3 1 + +#include +#include +#include + +#include +#include +#include +#include + +namespace bf = boost::fusion; +namespace qi = boost::spirit::qi; + +namespace gtsam { +typedef qi::grammar> base_grammar; + +struct QPSParser::MPSGrammar: base_grammar { + typedef std::vector Chars; + RawQP * rqp_; + boost::function const&)> setName; + boost::function const &)> addRow; + boost::function< + void(bf::vector const &)> rhsSingle; + boost::function< + void( + bf::vector)> rhsDouble; + boost::function< + void(bf::vector)> colSingle; + boost::function< + void( + bf::vector const &)> colDouble; + boost::function< + void(bf::vector const &)> addQuadTerm; + boost::function< + void( + bf::vector const &)> addBound; + boost::function< + void(bf::vector const &)> addBoundFr; + MPSGrammar(RawQP * rqp) : + base_grammar(start), rqp_(rqp), setName( + boost::bind(&RawQP::setName, rqp, ::_1)), addRow( + boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle( + boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble( + boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), colSingle( + boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble( + boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm( + boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound( + boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr( + boost::bind(&RawQP::addBoundFr, rqp, ::_1)) { + using namespace boost::spirit; + using namespace boost::spirit::qi; + character = lexeme[alnum | '_' | '-' | '.']; + title = lexeme[character >> *(blank | character)]; + word = lexeme[+character]; + name = lexeme[lit("NAME") >> *blank >> title >> +space][setName]; + row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow]; + rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ + >> *blank][rhsSingle]; + rhs_double = lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ + >> +blank >> word >> +blank >> double_)[rhsDouble] >> *blank]; + col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ + >> *blank][colSingle]; + col_double = lexeme[*blank + >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word + >> +blank >> double_)[colDouble] >> *blank]; + quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ + >> *blank][addQuadTerm]; + bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank + >> double_)[addBound] >> *blank]; + bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word + >> *blank][addBoundFr]; + rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)]; + rhs = lexeme[lit("RHS") >> *blank >> eol + >> +((rhs_double | rhs_single) >> eol)]; + cols = lexeme[lit("COLUMNS") >> *blank >> eol + >> +((col_double | col_single) >> eol)]; + quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)]; + bounds = lexeme[lit("BOUNDS") >> +space >> +((bound | bound_fr) >> eol)]; + ranges = lexeme[lit("RANGES") >> +space]; + end = lexeme[lit("ENDATA") >> *space]; + start = lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad + >> end]; + } + + qi::rule, char()> character; + qi::rule, Chars()> word, title; + qi::rule > row, end, col_single, + col_double, rhs_single, rhs_double, ranges, bound, bound_fr, bounds, quad, + quad_l, rows, cols, rhs, name, start; +}; + +QP QPSParser::Parse() { + RawQP rawData; + std::fstream stream(fileName_.c_str()); + stream.unsetf(std::ios::skipws); + boost::spirit::basic_istream_iterator begin(stream); + boost::spirit::basic_istream_iterator last; + + if (!parse(begin, last, MPSGrammar(&rawData)) || begin != last) { + throw QPSParserException(); + } + + return rawData.makeQP(); +} + +} diff --git a/gtsam_unstable/linear/QPSParser.h b/gtsam_unstable/linear/QPSParser.h new file mode 100644 index 000000000..088168829 --- /dev/null +++ b/gtsam_unstable/linear/QPSParser.h @@ -0,0 +1,40 @@ +/* ---------------------------------------------------------------------------- + + * 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 QPParser.h + * @brief QPS parser implementation + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class QPSParser { + +private: + std::string fileName_; + struct MPSGrammar; +public: + + QPSParser(const std::string& fileName) : + fileName_(findExampleDataFile(fileName)) { + } + + QP Parse(); +}; +} + diff --git a/gtsam_unstable/linear/QPSParserException.h b/gtsam_unstable/linear/QPSParserException.h new file mode 100644 index 000000000..ed4c79bdd --- /dev/null +++ b/gtsam_unstable/linear/QPSParserException.h @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + + * 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 QPSParserException.h + * @brief Exception thrown if there is an error parsing a QPS file + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#pragma once + +namespace gtsam { + +class QPSParserException: public ThreadsafeException { +public: + QPSParserException() { + } + + virtual ~QPSParserException() throw () { + } + + virtual const char *what() const throw () { + if (description_.empty()) + description_ = "There is a problem parsing the QPS file.\n"; + return description_.c_str(); + } + +private: + mutable std::string description_; +}; + +} + diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 0d9bbae6e..5a4ac79ca 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -1,252 +1,23 @@ -/* - * QPSolver.cpp - * @brief: - * @date: Apr 15, 2014 - * @author: thduynguyen +/* ---------------------------------------------------------------------------- + + * 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 QPSolver.cpp + * @brief + * @date Apr 15, 2014 + * @author Duy-Nguyen Ta */ -#include -#include #include -#include - -using namespace std; - namespace gtsam { - -//****************************************************************************** -QPSolver::QPSolver(const QP& qp) : qp_(qp) { - baseGraph_ = qp_.cost; - baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); - costVariableIndex_ = VariableIndex(qp_.cost); - equalityVariableIndex_ = VariableIndex(qp_.equalities); - inequalityVariableIndex_ = VariableIndex(qp_.inequalities); - constrainedKeys_ = qp_.equalities.keys(); - constrainedKeys_.merge(qp_.inequalities.keys()); -} - -//****************************************************************************** -VectorValues QPSolver::solveWithCurrentWorkingSet( - const LinearInequalityFactorGraph& workingSet) const { - GaussianFactorGraph workingGraph = baseGraph_; - BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { - if (factor->active()) - workingGraph.push_back(factor); - } - return workingGraph.optimize(); -} - -//****************************************************************************** -JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, - const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { - - // Transpose the A matrix of constrained factors to have the jacobian of the dual key - std::vector > Aterms = collectDualJacobians - < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); - std::vector > AtermsInequalities = collectDualJacobians - < LinearInequality > (key, workingSet, inequalityVariableIndex_); - Aterms.insert(Aterms.end(), AtermsInequalities.begin(), - AtermsInequalities.end()); - - // Collect the gradients of unconstrained cost factors to the b vector - if (Aterms.size() > 0) { - Vector b = Vector::Zero(delta.at(key).size()); - if (costVariableIndex_.find(key) != costVariableIndex_.end()) { - BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { - GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); - b += factor->gradient(key, delta); - } - } - return boost::make_shared(Aterms, b, noiseModel::Constrained::All(b.rows())); - } - else { - return boost::make_shared(); - } -} - -//****************************************************************************** -GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( - const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { - GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); - BOOST_FOREACH(Key key, constrainedKeys_) { - // Each constrained key becomes a factor in the dual graph - JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta); - if (!dualFactor->empty()) - dualGraph->push_back(dualFactor); - } - return dualGraph; -} - -//****************************************************************************** -int QPSolver::identifyLeavingConstraint( - const LinearInequalityFactorGraph& workingSet, - const VectorValues& lambdas) const { - int worstFactorIx = -1; - // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either - // inactive or a good inequality constraint, so we don't care! - double maxLambda = 0.0; - for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { - const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); - if (factor->active()) { - double lambda = lambdas.at(factor->dualKey())[0]; - if (lambda > maxLambda) { - worstFactorIx = factorIx; - maxLambda = lambda; - } - } - } - return worstFactorIx; -} - -//****************************************************************************** -/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints - * If some inactive inequality constraints complain about the full step (alpha = 1), - * we have to adjust alpha to stay within the inequality constraints' feasible regions. - * - * For each inactive inequality j: - * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints - * - We want: aj'*(xk + alpha*p) - bj <= 0 - * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 - * it's good! - * - We only care when aj'*p > 0. In this case, we need to choose alpha so that - * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) - * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) - * - * We want the minimum of all those alphas among all inactive inequality. - */ -boost::tuple QPSolver::computeStepSize( - const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const { - static bool debug = false; - - double minAlpha = 1.0; - int closestFactorIx = -1; - for(size_t factorIx = 0; factorIxgetb()[0]; - // only check inactive factors - if (!factor->active()) { - // Compute a'*p - double aTp = factor->dotProductRow(p); - - // Check if a'*p >0. Don't care if it's not. - if (aTp <= 0) - continue; - - // Compute a'*xk - double aTx = factor->dotProductRow(xk); - - // alpha = (b - a'*xk) / (a'*p) - double alpha = (b - aTx) / aTp; - if (debug) - cout << "alpha: " << alpha << endl; - - // We want the minimum of all those max alphas - if (alpha < minAlpha) { - closestFactorIx = factorIx; - minAlpha = alpha; - } - } - - } - - return boost::make_tuple(minAlpha, closestFactorIx); -} - -//****************************************************************************** -QPState QPSolver::iterate(const QPState& state) const { - static bool debug = false; - - // Solve with the current working set - VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); - if (debug) - newValues.print("New solution:"); - - // If we CAN'T move further - if (newValues.equals(state.values, 1e-5)) { - // Compute lambda from the dual graph - if (debug) - cout << "Building dual graph..." << endl; - GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); - if (debug) - dualGraph->print("Dual graph: "); - VectorValues duals = dualGraph->optimize(); - if (debug) - duals.print("Duals :"); - - int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); - if (debug) - cout << "leavingFactor: " << leavingFactor << endl; - - // If all inequality constraints are satisfied: We have the solution!! - if (leavingFactor < 0) { - return QPState(newValues, duals, state.workingSet, true); - } - else { - // Inactivate the leaving constraint - LinearInequalityFactorGraph newWorkingSet = state.workingSet; - newWorkingSet.at(leavingFactor)->inactivate(); - return QPState(newValues, duals, newWorkingSet, false); - } - } - else { - // If we CAN make some progress - // Adapt stepsize if some inactive constraints complain about this move - double alpha; - int factorIx; - VectorValues p = newValues - state.values; - boost::tie(alpha, factorIx) = // - computeStepSize(state.workingSet, state.values, p); - if (debug) - cout << "alpha, factorIx: " << alpha << " " << factorIx << " " - << endl; - - // also add to the working set the one that complains the most - LinearInequalityFactorGraph newWorkingSet = state.workingSet; - if (factorIx >= 0) - newWorkingSet.at(factorIx)->activate(); - - // step! - newValues = state.values + alpha * p; - - return QPState(newValues, state.duals, newWorkingSet, false); - } -} - -//****************************************************************************** -LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( - const LinearInequalityFactorGraph& inequalities, - const VectorValues& initialValues) const { - LinearInequalityFactorGraph workingSet; - BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){ - LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); - double error = workingFactor->error(initialValues); - if (fabs(error)>1e-7){ - workingFactor->inactivate(); - } else { - workingFactor->activate(); - } - workingSet.push_back(workingFactor); - } - return workingSet; -} - -//****************************************************************************** -pair QPSolver::optimize( - const VectorValues& initialValues) const { - - // Initialize workingSet from the feasible initialValues - LinearInequalityFactorGraph workingSet = - identifyActiveConstraints(qp_.inequalities, initialValues); - QPState state(initialValues, VectorValues(), workingSet, false); - - /// main loop of the solver - while (!state.converged) { - state = iterate(state); - } - - return make_pair(state.values, state.duals); -} - -} /* namespace gtsam */ +constexpr double QPPolicy::maxAlpha; +} \ No newline at end of file diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 68713f965..9efc23a67 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -1,188 +1,43 @@ -/* - * QPSolver.h - * @brief: A quadratic programming solver implements the active set method - * @date: Apr 15, 2014 - * @author: thduynguyen +/* ---------------------------------------------------------------------------- + + * 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 QPSolver.h + * @brief Policy of ActiveSetSolver to solve Quadratic Programming Problems + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 6/16/16 */ -#pragma once - -#include #include - -#include -#include +#include +#include +#include +#include namespace gtsam { -/// This struct holds the state of QPSolver at each iteration -struct QPState { - VectorValues values; - VectorValues duals; - LinearInequalityFactorGraph workingSet; - bool converged; +/// Policy for ActivetSetSolver to solve Linear Programming \sa QP problems +struct QPPolicy { + /// Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient + /// For QP, maxAlpha = 1 is the minimum point of the quadratic cost + static constexpr double maxAlpha = 1.0; - /// default constructor - QPState() : - values(), duals(), workingSet(), converged(false) { - } - - /// constructor with initial values - QPState(const VectorValues& initialValues, const VectorValues& initialDuals, - const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) : - values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( - _converged) { + /// Simply the cost of the QP problem + static const GaussianFactorGraph& buildCostFunction( + const QP& qp, const VectorValues& xk = VectorValues()) { + return qp.cost; } }; -/** - * This class implements the active set method to solve quadratic programming problems - * encoded in a GaussianFactorGraph with special mixed constrained noise models, in which - * a negative sigma denotes an inequality <=0 constraint, - * a zero sigma denotes an equality =0 constraint, - * and a positive sigma denotes a normal Gaussian noise model. - */ -class QPSolver { +using QPSolver = ActiveSetSolver; - const QP& qp_; //!< factor graphs of the QP problem, can't be modified! - GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. - VariableIndex costVariableIndex_, equalityVariableIndex_, - inequalityVariableIndex_; - KeySet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph - -public: - /// Constructor - QPSolver(const QP& qp); - - /// Find solution with the current working set - VectorValues solveWithCurrentWorkingSet( - const LinearInequalityFactorGraph& workingSet) const; - - /// @name Build the dual graph - /// @{ - - /// Collect the Jacobian terms for a dual factor - template - std::vector > collectDualJacobians(Key key, - const FactorGraph& graph, - const VariableIndex& variableIndex) const { - std::vector > Aterms; - if (variableIndex.find(key) != variableIndex.end()) { - BOOST_FOREACH(size_t factorIx, variableIndex[key]){ - typename FACTOR::shared_ptr factor = graph.at(factorIx); - if (!factor->active()) continue; - Matrix Ai = factor->getA(factor->find(key)).transpose(); - Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); - } - } - return Aterms; - } - - /// Create a dual factor - JacobianFactor::shared_ptr createDualFactor(Key key, - const LinearInequalityFactorGraph& workingSet, - const VectorValues& delta) const; - - /** - * Build the dual graph to solve for the Lagrange multipliers. - * - * The Lagrangian function is: - * L(X,lambdas) = f(X) - \sum_k lambda_k * c_k(X), - * where the unconstrained part is - * f(X) = 0.5*X'*G*X - X'*g + 0.5*f0 - * and the linear equality constraints are - * c1(X), c2(X), ..., cm(X) - * - * Take the derivative of L wrt X at the solution and set it to 0, we have - * \grad f(X) = \sum_k lambda_k * \grad c_k(X) (*) - * - * For each set of rows of (*) corresponding to a variable xi involving in some constraints - * we have: - * \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi - * \grad c_k(xi) = \frac{\partial c_k}{\partial xi}' - * - * Note: If xi does not involve in any constraint, we have the trivial condition - * \grad f(Xi) = 0, which should be satisfied as a usual condition for unconstrained variables. - * - * So each variable xi involving in some constraints becomes a linear factor A*lambdas - b = 0 - * on the constraints' lambda multipliers, as follows: - * - The jacobian term A_k for each lambda_k is \grad c_k(xi) - * - The constant term b is \grad f(xi), which can be computed from all unconstrained - * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi - */ - GaussianFactorGraph::shared_ptr buildDualGraph( - const LinearInequalityFactorGraph& workingSet, - const VectorValues& delta) const; - - /// @} - - /** - * The goal of this function is to find currently active inequality constraints - * that violate the condition to be active. The one that violates the condition - * the most will be removed from the active set. See Nocedal06book, pg 469-471 - * - * Find the BAD active inequality that pulls x strongest to the wrong direction - * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0) - * - * For active inequality constraints (those that are enforced as equality constraints - * in the current working set), we want lambda < 0. - * This is because: - * - From the Lagrangian L = f - lambda*c, we know that the constraint force - * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay - * on the constraint surface, the constraint force has to balance out with - * other unconstrained forces that are pulling x towards the unconstrained - * minimum point. The other unconstrained forces are pulling x toward (-\grad f), - * hence the constraint force has to be exactly \grad f, so that the total - * force is 0. - * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0), - * while we are solving for - (<=0) constraint. - * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction - * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied. - * That means we want lambda < 0. - * - This is because when the constrained force pulls x towards the infeasible region (+), - * the unconstrained force is pulling x towards the opposite direction into - * the feasible region (again because the total force has to be 0 to make x stay still) - * So we can drop this constraint to have a lower error but feasible solution. - * - * In short, active inequality constraints with lambda > 0 are BAD, because they - * violate the condition to be active. - * - * And we want to remove the worst one with the largest lambda from the active set. - * - */ - int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet, - const VectorValues& lambdas) const; - - /** - * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] - * - * @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex) - * is the constraint that has minimum alpha, or (-1,-1) if alpha = 1. - * This constraint will be added to the working set and become active - * in the next iteration - */ - boost::tuple computeStepSize( - const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const; - - /** Iterate 1 step, return a new state with a new workingSet and values */ - QPState iterate(const QPState& state) const; - - /** - * Identify active constraints based on initial values. - */ - LinearInequalityFactorGraph identifyActiveConstraints( - const LinearInequalityFactorGraph& inequalities, - const VectorValues& initialValues) const; - - /** Optimize with a provided initial values - * For this version, it is the responsibility of the caller to provide - * a feasible initial value. - * @return a pair of solutions - */ - std::pair optimize( - const VectorValues& initialValues) const; - -}; - -} /* namespace gtsam */ +} \ No newline at end of file diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp new file mode 100644 index 000000000..ec71cae5b --- /dev/null +++ b/gtsam_unstable/linear/RawQP.cpp @@ -0,0 +1,271 @@ +/* ---------------------------------------------------------------------------- + + * 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 RawQP.cpp + * @brief + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#include +#include + +using boost::fusion::at_c; + +namespace gtsam { + +void RawQP::setName( + boost::fusion::vector, std::vector, + std::vector> const &name) { + name_ = std::string(at_c < 1 > (name).begin(), at_c < 1 > (name).end()); + if (debug) { + std::cout << "Parsing file: " << name_ << std::endl; + } +} + +void RawQP::addColumn( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const &vars) { + + std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + Matrix11 coefficient = at_c < 5 > (vars) * I_1x1; + + if (!varname_to_key.count(var_)) + varname_to_key[var_] = Symbol('X', varNumber++); + if (row_ == obj_name) { + g[varname_to_key[var_]] = coefficient; + return; + } + (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient; + if (debug) { + std::cout << "Added Column for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << std::endl; + } + +} + +void RawQP::addColumnDouble( + boost::fusion::vector, std::vector, + std::vector, std::vector, double, std::vector, + std::vector, std::vector, double> const &vars) { + + std::string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end()); + std::string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end()); + std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end()); + Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1; + Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1; + if (!varname_to_key.count(var_)) + varname_to_key.insert( { var_, Symbol('X', varNumber++) }); + if (row1_ == obj_name) + g[varname_to_key[var_]] = coefficient1; + else + (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1; + if (row2_ == obj_name) + g[varname_to_key[var_]] = coefficient2; + else + (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2; +} + +void RawQP::addRHS( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const &vars) { + + std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + double coefficient = at_c < 5 > (vars); + if (row_ == obj_name) + f = -coefficient; + else + b[row_] = coefficient; + + if (debug) { + std::cout << "Added RHS for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << std::endl; + } +} + +void RawQP::addRHSDouble( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector, std::vector, std::vector, double> const &vars) { + + std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end()); + double coefficient1 = at_c < 5 > (vars); + double coefficient2 = at_c < 9 > (vars); + if (row1_ == obj_name) + f = -coefficient1; + else + b[row1_] = coefficient1; + + if (row2_ == obj_name) + f = -coefficient2; + else + b[row2_] = coefficient2; + + if (debug) { + std::cout << "Added RHS for Var: " << var_ << " Row: " << row1_ + << " Coefficient: " << coefficient1 << std::endl; + std::cout << " " << "Row: " << row2_ + << " Coefficient: " << coefficient2 << std::endl; + } +} + +void RawQP::addRow( + boost::fusion::vector, char, std::vector, + std::vector, std::vector> const &vars) { + + std::string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + char type = at_c < 1 > (vars); + switch (type) { + case 'N': + obj_name = name_; + break; + case 'L': + row_to_constraint_v[name_] = &IL; + break; + case 'G': + row_to_constraint_v[name_] = &IG; + break; + case 'E': + row_to_constraint_v[name_] = &E; + break; + default: + std::cout << "invalid type: " << type << std::endl; + break; + } + if (debug) { + std::cout << "Added Row Type: " << type << " Name: " << name_ << std::endl; + } +} + +void RawQP::addBound( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector, double> const &vars) { + + std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); + double number = at_c < 7 > (vars); + if (type_.compare(std::string("UP")) == 0) + up[varname_to_key[var_]] = number; + else if (type_.compare(std::string("LO")) == 0) + lo[varname_to_key[var_]] = number; + else + std::cout << "Invalid Bound Type: " << type_ << std::endl; + + if (debug) { + std::cout << "Added Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << number << std::endl; + } +} + +void RawQP::addBoundFr( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector> const &vars) { + std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); + Free.push_back(varname_to_key[var_]); + if (debug) { + std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << std::endl; + } +} + +void RawQP::addQuadTerm( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const &vars) { + std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + Matrix11 coefficient = at_c < 5 > (vars) * I_1x1; + + H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient; + H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient; + if (debug) { + std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ + << " Coefficient: " << coefficient << std::endl; + } +} + +QP RawQP::makeQP() { + std::vector < Key > keys; + std::vector < Matrix > Gs; + std::vector < Vector > gs; + for (auto kv : varname_to_key) { + keys.push_back(kv.second); + } + std::sort(keys.begin(), keys.end()); + for (unsigned int i = 0; i < keys.size(); ++i) { + for (unsigned int j = i; j < keys.size(); ++j) { + Gs.push_back(H[keys[i]][keys[j]]); + } + } + for (Key key1 : keys) { + gs.push_back(-g[key1]); + } + int dual_key_num = keys.size() + 1; + QP madeQP; + auto obj = HessianFactor(keys, Gs, gs, f); + + madeQP.cost.push_back(obj); + + for (auto kv : E) { + std::map keyMatrixMap; + for (auto km : kv.second) { + keyMatrixMap.insert(km); + } + madeQP.equalities.push_back( + LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++)); + } + + for (auto kv : IG) { + std::map keyMatrixMap; + for (auto km : kv.second) { + km.second = -km.second; + keyMatrixMap.insert(km); + } + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++)); + } + + for (auto kv : IL) { + std::map keyMatrixMap; + for (auto km : kv.second) { + keyMatrixMap.insert(km); + } + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++)); + } + + for (Key k : keys) { + if (std::find(Free.begin(), Free.end(), k) != Free.end()) + continue; + if (up.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, I_1x1, up[k], dual_key_num++)); + if (lo.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, -I_1x1, lo[k], dual_key_num++)); + else + madeQP.inequalities.push_back( + LinearInequality(k, -I_1x1, 0, dual_key_num++)); + } + return madeQP; +} +} + diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h new file mode 100644 index 000000000..aadf11e50 --- /dev/null +++ b/gtsam_unstable/linear/RawQP.h @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * 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 RawQP.h + * @brief + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +class RawQP { +private: + typedef std::unordered_map coefficient_v; + typedef std::unordered_map constraint_v; + + std::unordered_map row_to_constraint_v; + constraint_v E; + constraint_v IG; + constraint_v IL; + unsigned int varNumber; + std::unordered_map b; + std::unordered_map g; + std::unordered_map varname_to_key; + std::unordered_map > H; + double f; + std::string obj_name; + std::string name_; + std::unordered_map up; + std::unordered_map lo; + std::vector Free; + const bool debug = false; + +public: + RawQP() : + row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() { + } + + void setName( + boost::fusion::vector, std::vector, + std::vector> const & name); + + void addColumn( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const & vars); + + void addColumnDouble( + boost::fusion::vector, std::vector, + std::vector, std::vector, double, std::vector, + std::vector, std::vector, double> const & vars); + + void addRHS( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const & vars); + + void addRHSDouble( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector, std::vector, std::vector, double> const & vars); + + void addRow( + boost::fusion::vector, char, std::vector, + std::vector, std::vector> const & vars); + + void addBound( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector, double> const & vars); + + void addBoundFr( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector> const & vars); + + void addQuadTerm( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const & vars); + + QP makeQP(); +} +; +} diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp new file mode 100644 index 000000000..a105a39f0 --- /dev/null +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -0,0 +1,255 @@ +/* ---------------------------------------------------------------------------- + + * 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 testQPSolver.cpp + * @brief Test simple QP solver for a linear inequality constraint + * @date Apr 10, 2014 + * @author Duy-Nguyen Ta + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::symbol_shorthand; + +static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1); + +/* ************************************************************************* */ +/** + * min -x1-x2 + * s.t. x1 + 2x2 <= 4 + * 4x1 + 2x2 <= 12 + * -x1 + x2 <= 1 + * x1, x2 >= 0 + */ +LP simpleLP1() { + LP lp; + lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2) + lp.inequalities.push_back( + LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0 + lp.inequalities.push_back( + LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0 + lp.inequalities.push_back( + LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4 + lp.inequalities.push_back( + LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12 + lp.inequalities.push_back( + LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1 + return lp; +} + +/* ************************************************************************* */ +namespace gtsam { + +TEST(LPInitSolver, infinite_loop_single_var) { + LP initchecker; + initchecker.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha + initchecker.inequalities.push_back( + LinearInequality(1, Vector3(-2, -1, -1), -2, 1)); //-2x-y-alpha <= -2 + initchecker.inequalities.push_back( + LinearInequality(1, Vector3(-1, 2, -1), 6, 2)); // -x+2y-alpha <= 6 + initchecker.inequalities.push_back( + LinearInequality(1, Vector3(-1, 0, -1), 0, 3)); // -x - alpha <= 0 + initchecker.inequalities.push_back( + LinearInequality(1, Vector3(1, 0, -1), 20, 4)); // x - alpha <= 20 + initchecker.inequalities.push_back( + LinearInequality(1, Vector3(0, -1, -1), 0, 5)); // -y - alpha <= 0 + LPSolver solver(initchecker); + VectorValues starter; + starter.insert(1, Vector3(0, 0, 2)); + VectorValues results, duals; + boost::tie(results, duals) = solver.optimize(starter); + VectorValues expected; + expected.insert(1, Vector3(13.5, 6.5, -6.5)); + CHECK(assert_equal(results, expected, 1e-7)); +} + +TEST(LPInitSolver, infinite_loop_multi_var) { + LP initchecker; + Key X = symbol('X', 1); + Key Y = symbol('Y', 1); + Key Z = symbol('Z', 1); + initchecker.cost = LinearCost(Z, kOne); // min alpha + initchecker.inequalities.push_back( + LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2, + 1)); //-2x-y-alpha <= -2 + initchecker.inequalities.push_back( + LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6, + 2)); // -x+2y-alpha <= 6 + initchecker.inequalities.push_back(LinearInequality( + X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0 + initchecker.inequalities.push_back(LinearInequality( + X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20 + initchecker.inequalities.push_back(LinearInequality( + Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0 + LPSolver solver(initchecker); + VectorValues starter; + starter.insert(X, kZero); + starter.insert(Y, kZero); + starter.insert(Z, Vector::Constant(1, 2.0)); + VectorValues results, duals; + boost::tie(results, duals) = solver.optimize(starter); + VectorValues expected; + expected.insert(X, Vector::Constant(1, 13.5)); + expected.insert(Y, Vector::Constant(1, 6.5)); + expected.insert(Z, Vector::Constant(1, -6.5)); + CHECK(assert_equal(results, expected, 1e-7)); +} + +TEST(LPInitSolver, initialization) { + LP lp = simpleLP1(); + LPInitSolver initSolver(lp); + + GaussianFactorGraph::shared_ptr initOfInitGraph = + initSolver.buildInitOfInitGraph(); + VectorValues x0 = initOfInitGraph->optimize(); + VectorValues expected_x0; + expected_x0.insert(1, Vector::Zero(2)); + CHECK(assert_equal(expected_x0, x0, 1e-10)); + + double y0 = initSolver.compute_y0(x0); + double expected_y0 = 0.0; + DOUBLES_EQUAL(expected_y0, y0, 1e-7); + + Key yKey = 2; + LP::shared_ptr initLP = initSolver.buildInitialLP(yKey); + LP expectedInitLP; + expectedInitLP.cost = LinearCost(yKey, kOne); + expectedInitLP.inequalities.push_back(LinearInequality( + 1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0 + expectedInitLP.inequalities.push_back(LinearInequality( + 1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0 + expectedInitLP.inequalities.push_back( + LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4, + 3)); // x1 + 2*x2 - y <= 4 + expectedInitLP.inequalities.push_back( + LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12, + 4)); // 4x1 + 2x2 - y <= 12 + expectedInitLP.inequalities.push_back( + LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1, + 5)); // -x1 + x2 - y <= 1 + CHECK(assert_equal(expectedInitLP, *initLP, 1e-10)); + LPSolver lpSolveInit(*initLP); + VectorValues xy0(x0); + xy0.insert(yKey, Vector::Constant(1, y0)); + VectorValues xyInit = lpSolveInit.optimize(xy0).first; + VectorValues expected_init; + expected_init.insert(1, Vector::Ones(2)); + expected_init.insert(2, Vector::Constant(1, -1)); + CHECK(assert_equal(expected_init, xyInit, 1e-10)); + + VectorValues x = initSolver.solve(); + CHECK(lp.isFeasible(x)); +} +} + +/* ************************************************************************* */ +/** + * TEST gtsam solver with an over-constrained system + * x + y = 1 + * x - y = 5 + * x + 2y = 6 + */ +TEST(LPSolver, overConstrainedLinearSystem) { + GaussianFactorGraph graph; + Matrix A1 = Vector3(1, 1, 1); + Matrix A2 = Vector3(1, -1, 2); + Vector b = Vector3(1, 5, 6); + JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); + graph.push_back(factor); + + VectorValues x = graph.optimize(); + // This check confirms that gtsam linear constraint solver can't handle + // over-constrained system + CHECK(factor.error(x) != 0.0); +} + +TEST(LPSolver, overConstrainedLinearSystem2) { + GaussianFactorGraph graph; + graph.emplace_shared(1, I_1x1, 2, I_1x1, kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, -I_1x1, 5 * kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, + noiseModel::Constrained::All(1)); + VectorValues x = graph.optimize(); + // This check confirms that gtsam linear constraint solver can't handle + // over-constrained system + CHECK(graph.error(x) != 0.0); +} + +/* ************************************************************************* */ +TEST(LPSolver, simpleTest1) { + LP lp = simpleLP1(); + LPSolver lpSolver(lp); + VectorValues init; + init.insert(1, Vector::Zero(2)); + + VectorValues x1 = + lpSolver.buildWorkingGraph(InequalityFactorGraph(), init).optimize(); + VectorValues expected_x1; + expected_x1.insert(1, Vector::Ones(2)); + CHECK(assert_equal(expected_x1, x1, 1e-10)); + + VectorValues result, duals; + boost::tie(result, duals) = lpSolver.optimize(init); + VectorValues expectedResult; + expectedResult.insert(1, Vector2(8. / 3., 2. / 3.)); + CHECK(assert_equal(expectedResult, result, 1e-10)); +} + +/* ************************************************************************* */ +TEST(LPSolver, testWithoutInitialValues) { + LP lp = simpleLP1(); + LPSolver lpSolver(lp); + VectorValues result, duals, expectedResult; + expectedResult.insert(1, Vector2(8. / 3., 2. / 3.)); + boost::tie(result, duals) = lpSolver.optimize(); + CHECK(assert_equal(expectedResult, result)); +} + +/** + * TODO: More TEST cases: + * - Infeasible + * - Unbounded + * - Underdetermined + */ +/* ************************************************************************* */ +TEST(LPSolver, LinearCost) { + LinearCost cost(1, Vector3(2., 4., 6.)); + VectorValues x; + x.insert(1, Vector3(1., 3., 5.)); + double error = cost.error(x); + double expectedError = 44.0; + DOUBLES_EQUAL(expectedError, error, 1e-100); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index 90f3de586..fa94dd255 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -12,7 +12,7 @@ /** * @file testLinearEquality.cpp * @brief Unit tests for LinearEquality - * @author thduynguyen + * @author Duy-Nguyen Ta **/ #include @@ -28,20 +28,20 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -GTSAM_CONCEPT_TESTABLE_INST(LinearEquality) +GTSAM_CONCEPT_TESTABLE_INST (LinearEquality) namespace { - namespace simple { - // Terms we'll use - const vector > terms = list_of > - (make_pair(5, Matrix3::Identity())) - (make_pair(10, 2*Matrix3::Identity())) - (make_pair(15, 3*Matrix3::Identity())); +namespace simple { +// Terms we'll use +const vector > terms = list_of < pair + > (make_pair(5, Matrix3::Identity()))( + make_pair(10, 2 * Matrix3::Identity()))( + make_pair(15, 3 * Matrix3::Identity())); - // RHS and sigmas - const Vector b = (Vector(3) << 1., 2., 3.).finished(); - const SharedDiagonal noise = noiseModel::Constrained::All(3); - } +// RHS and sigmas +const Vector b = (Vector(3) << 1., 2., 3.).finished(); +const SharedDiagonal noise = noiseModel::Constrained::All(3); +} } /* ************************************************************************* */ @@ -53,7 +53,7 @@ TEST(LinearEquality, constructors_and_accessors) { // One term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); LinearEquality actual(terms[0].first, terms[0].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); @@ -65,9 +65,9 @@ TEST(LinearEquality, constructors_and_accessors) { // Two term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); LinearEquality actual(terms[0].first, terms[0].second, - terms[1].first, terms[1].second, b, 0); + terms[1].first, terms[1].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); @@ -78,9 +78,9 @@ TEST(LinearEquality, constructors_and_accessors) { // Three term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); LinearEquality actual(terms[0].first, terms[0].second, - terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); @@ -93,10 +93,10 @@ TEST(LinearEquality, constructors_and_accessors) /* ************************************************************************* */ TEST(LinearEquality, Hessian_conversion) { HessianFactor hessian(0, (Matrix(4,4) << - 1.57, 2.695, -1.1, -2.35, - 2.695, 11.3125, -0.65, -10.225, - -1.1, -0.65, 1, 0.5, - -2.35, -10.225, 0.5, 9.25).finished(), + 1.57, 2.695, -1.1, -2.35, + 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, + -2.35, -10.225, 0.5, 9.25).finished(), (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725); @@ -169,8 +169,8 @@ TEST(LinearEquality, matrices) augmentedJacobianExpected << jacobianExpected, rhsExpected; Matrix augmentedHessianExpected = - augmentedJacobianExpected.transpose() * simple::noise->R().transpose() - * simple::noise->R() * augmentedJacobianExpected; + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() + * simple::noise->R() * augmentedJacobianExpected; // Whitened Jacobian EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); @@ -210,8 +210,8 @@ TEST(LinearEquality, operators ) // test gradient at zero Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); VectorValues expectedG; - expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); - expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); + expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); + expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); VectorValues actualG = lf.gradientAtZero(); EXPECT(assert_equal(expectedG, actualG)); } @@ -233,5 +233,8 @@ TEST(LinearEquality, empty ) } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 7442540f5..bc9dd1f98 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -19,14 +19,14 @@ #include #include #include - +#include #include using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -const Matrix One = I_1x1; +static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1); /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 @@ -37,15 +37,17 @@ QP createTestCase() { // Note the Hessian encodes: // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 + //TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation + // Should be 0.5x'Gx + gx + f : Nocedal 449 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1, - 2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0)); + HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1, + Z_1x1, 10.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 return qp; } @@ -53,8 +55,8 @@ QP createTestCase() { TEST(QPSolver, TestCase) { VectorValues values; double x1 = 5, x2 = 7; - values.insert(X(1), x1 * Matrix::Ones(1, 1)); - values.insert(X(2), x2 * Matrix::Ones(1, 1)); + values.insert(X(1), x1 * I_1x1); + values.insert(X(2), x2 * I_1x1); QP qp = createTestCase(); DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9); DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9); @@ -67,15 +69,15 @@ TEST(QPSolver, constraintsAux) { VectorValues lambdas; lambdas.insert(0, (Vector(1) << -0.5).finished()); - lambdas.insert(1, (Vector(1) << 0.0).finished()); - lambdas.insert(2, (Vector(1) << 0.3).finished()); - lambdas.insert(3, (Vector(1) << 0.1).finished()); + lambdas.insert(1, kZero); + lambdas.insert(2, (Vector(1) << 0.3).finished()); + lambdas.insert(3, (Vector(1) << 0.1).finished()); int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas); LONGS_EQUAL(2, factorIx); VectorValues lambdas2; lambdas2.insert(0, (Vector(1) << -0.5).finished()); - lambdas2.insert(1, (Vector(1) << 0.0).finished()); + lambdas2.insert(1, kZero); lambdas2.insert(2, (Vector(1) << -0.3).finished()); lambdas2.insert(3, (Vector(1) << -0.1).finished()); int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2); @@ -92,14 +94,14 @@ QP createEqualityConstrainedTest() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1, - 2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0)); + HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1, + 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector - Matrix A1 = (Matrix(1, 1) << 1).finished(); - Matrix A2 = (Matrix(1, 1) << 1).finished(); - Vector b = -(Vector(1) << 1).finished(); + Matrix A1 = I_1x1; + Matrix A2 = I_1x1; + Vector b = -kOne; qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); return qp; @@ -132,20 +134,19 @@ TEST(QPSolver, indentifyActiveConstraints) { currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(2), Z_1x1); - LinearInequalityFactorGraph workingSet = - solver.identifyActiveConstraints(qp.inequalities, currentSolution); + InequalityFactorGraph workingSet = solver.identifyActiveConstraints( + qp.inequalities, currentSolution); - CHECK(!workingSet.at(0)->active()); // inactive - CHECK(workingSet.at(1)->active()); // active - CHECK(workingSet.at(2)->active()); // active - CHECK(!workingSet.at(3)->active()); // inactive + CHECK(!workingSet.at(0)->active()); // inactive + CHECK(workingSet.at(1)->active());// active + CHECK(workingSet.at(2)->active());// active + CHECK(!workingSet.at(3)->active());// inactive - VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); + VectorValues solution = solver.buildWorkingGraph(workingSet).optimize(); VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 0.0).finished()); - expectedSolution.insert(X(2), (Vector(1) << 0.0).finished()); + expectedSolution.insert(X(1), kZero); + expectedSolution.insert(X(2), kZero); CHECK(assert_equal(expectedSolution, solution, 1e-100)); - } /* ************************************************************************* */ @@ -158,13 +159,13 @@ TEST(QPSolver, iterate) { currentSolution.insert(X(2), Z_1x1); std::vector expectedSolutions(4), expectedDuals(4); - expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished()); - expectedSolutions[0].insert(X(2), (Vector(1) << 0.0).finished()); + expectedSolutions[0].insert(X(1), kZero); + expectedSolutions[0].insert(X(2), kZero); expectedDuals[0].insert(1, (Vector(1) << 3).finished()); - expectedDuals[0].insert(2, (Vector(1) << 0).finished()); + expectedDuals[0].insert(2, kZero); expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished()); + expectedSolutions[1].insert(X(2), kZero); expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished()); @@ -173,10 +174,11 @@ TEST(QPSolver, iterate) { expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); - LinearInequalityFactorGraph workingSet = - solver.identifyActiveConstraints(qp.inequalities, currentSolution); + InequalityFactorGraph workingSet = solver.identifyActiveConstraints( + qp.inequalities, currentSolution); - QPState state(currentSolution, VectorValues(), workingSet, false); + QPSolver::State state(currentSolution, VectorValues(), workingSet, false, + 100); int it = 0; while (!state.converged) { @@ -209,6 +211,64 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { CHECK(assert_equal(expectedSolution, solution, 1e-100)); } +pair testParser(QPSParser parser) { + QP exampleqp = parser.Parse(); + QP expectedqp; + Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); + // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 + expectedqp.cost.push_back( + HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, + 2.0 * kOne, 4.0)); + // 2x + y >= 2 + // -x + 2y <= 6 + expectedqp.inequalities.push_back( + LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0)); + expectedqp.inequalities.push_back( + LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1)); + // x<= 20 + expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4)); + //x >= 0 + expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2)); + // y > = 0 + expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3)); + return std::make_pair(expectedqp, exampleqp); +} +; + +TEST(QPSolver, ParserSyntaticTest) { + auto expectedActual = testParser(QPSParser("QPExample.QPS")); + CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, + 1e-7)); + CHECK(assert_equal(expectedActual.first.inequalities, + expectedActual.second.inequalities, 1e-7)); + CHECK(assert_equal(expectedActual.first.equalities, + expectedActual.second.equalities, 1e-7)); +} + +TEST(QPSolver, ParserSemanticTest) { + auto expected_actual = testParser(QPSParser("QPExample.QPS")); + VectorValues actualSolution, expectedSolution; + boost::tie(expectedSolution, boost::tuples::ignore) = + QPSolver(expected_actual.first).optimize(); + boost::tie(actualSolution, boost::tuples::ignore) = + QPSolver(expected_actual.second).optimize(); + CHECK(assert_equal(actualSolution, expectedSolution, 1e-7)); +} + +TEST(QPSolver, QPExampleTest){ + QP problem = QPSParser("QPExample.QPS").Parse(); + VectorValues actualSolution; + auto solver = QPSolver(problem); + boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize(); + VectorValues expectedSolution; + expectedSolution.insert(Symbol('X',1),0.7625*I_1x1); + expectedSolution.insert(Symbol('X',2),0.4750*I_1x1); + double error_expected = problem.cost.error(expectedSolution); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(expectedSolution, actualSolution, 1e-7)) + CHECK(assert_equal(error_expected, error_actual)) +} + /* ************************************************************************* */ // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() { @@ -219,19 +279,22 @@ QP createTestMatlabQPEx() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 1.0 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1, - 2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 1000.0)); + HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, + 6 * I_1x1, 1000.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 - qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2 - qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3 - qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 + qp.inequalities.push_back( + LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2 + qp.inequalities.push_back( + LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3 + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0 return qp; } +///* ************************************************************************* */ TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); @@ -246,20 +309,35 @@ TEST(QPSolver, optimizeMatlabEx) { CHECK(assert_equal(expectedSolution, solution, 1e-7)); } +///* ************************************************************************* */ +TEST(QPSolver, optimizeMatlabExNoinitials) { + QP qp = createTestMatlabQPEx(); + QPSolver solver(qp); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-7)); +} + /* ************************************************************************* */ // Create test graph as in Nocedal06book, Ex 16.4, pg. 475 QP createTestNocedal06bookEx16_4() { QP qp; - qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), I_1x1)); - qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1)); + qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1)); + qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); - qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1)); - qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2)); - qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3)); - qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4)); + qp.inequalities.push_back( + LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0)); + qp.inequalities.push_back( + LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1)); + qp.inequalities.push_back( + LinearInequality(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2)); + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0.0, 3)); + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0.0, 4)); return qp; } @@ -280,28 +358,45 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { } /* ************************************************************************* */ - TEST(QPSolver, failedSubproblem) { QP qp; qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); qp.inequalities.push_back( - LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); + LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0)); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); VectorValues initialValues; - initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); + initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); QPSolver solver(qp); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); -// graph.print("Graph: "); -// solution.print("Solution: "); + CHECK(assert_equal(expected, solution, 1e-7)); } +/* ************************************************************************* */ +TEST(QPSolver, infeasibleInitial) { + QP qp; + qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2))); + qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0)); + qp.inequalities.push_back( + LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0)); + + VectorValues expected; + expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); + + VectorValues initialValues; + initialValues.insert(X(1), (Vector(2) << -10.0, 100.0).finished()); + + QPSolver solver(qp); + VectorValues solution; + CHECK_EXCEPTION(solver.optimize(initialValues), InfeasibleInitialValues); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 470701d97..a0ede5f74 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -22,13 +22,13 @@ #include #include #include -//#include -#include + +using namespace std; namespace gtsam { /* ************************************************************************* */ -void BatchFixedLagSmoother::print(const std::string& s, +void BatchFixedLagSmoother::print(const string& s, const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? @@ -45,7 +45,7 @@ bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, /* ************************************************************************* */ Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { - throw std::runtime_error( + throw runtime_error( "BatchFixedLagSmoother::marginalCovariance not implemented"); } @@ -54,17 +54,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { - const bool debug = ISDEBUG("BatchFixedLagSmoother update"); - if (debug) { - std::cout << "BatchFixedLagSmoother::update() START" << std::endl; - } - // Update all of the internal variables with the new information gttic(augment_system); // Add the new variables to theta theta_.insert(newTheta); // Add new variables to the end of the ordering - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + for (const auto& key_value : newTheta) { ordering_.push_back(key_value.key); } // Augment Delta @@ -79,19 +74,10 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update( // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if (debug) - std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore( + KeyVector marginalizableKeys = findKeysBefore( current_timestamp - smootherLag_); - if (debug) { - std::cout << "Marginalizable Keys: "; - BOOST_FOREACH(Key key, marginalizableKeys) { - std::cout << DefaultKeyFormatter(key) << " "; - } - std::cout << std::endl; - } // Reorder gttic(reorder); @@ -113,17 +99,13 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update( } gttoc(marginalize); - if (debug) { - std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl; - } - return result; } /* ************************************************************************* */ void BatchFixedLagSmoother::insertFactors( const NonlinearFactorGraph& newFactors) { - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { + for(const auto& factor: newFactors) { Key index; // Insert the factor into an existing hole in the factor graph, if possible if (availableSlots_.size() > 0) { @@ -135,7 +117,7 @@ void BatchFixedLagSmoother::insertFactors( factors_.push_back(factor); } // Update the FactorIndex - BOOST_FOREACH(Key key, *factor) { + for(Key key: *factor) { factorIndex_[key].insert(index); } } @@ -143,11 +125,11 @@ void BatchFixedLagSmoother::insertFactors( /* ************************************************************************* */ void BatchFixedLagSmoother::removeFactors( - const std::set& deleteFactors) { - BOOST_FOREACH(size_t slot, deleteFactors) { + const set& deleteFactors) { + for(size_t slot: deleteFactors) { if (factors_.at(slot)) { // Remove references to this factor from the FactorIndex - BOOST_FOREACH(Key key, *(factors_.at(slot))) { + for(Key key: *(factors_.at(slot))) { factorIndex_[key].erase(slot); } // Remove the factor from the factor graph @@ -156,16 +138,16 @@ void BatchFixedLagSmoother::removeFactors( availableSlots_.push(slot); } else { // TODO: Throw an error?? - std::cout << "Attempting to remove a factor from slot " << slot - << ", but it is already NULL." << std::endl; + cout << "Attempting to remove a factor from slot " << slot + << ", but it is already NULL." << endl; } } } /* ************************************************************************* */ -void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { +void BatchFixedLagSmoother::eraseKeys(const KeyVector& keys) { - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { // Erase the key from the values theta_.erase(key); @@ -181,51 +163,21 @@ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { eraseKeyTimestampMap(keys); // Remove marginalized keys from the ordering and delta - BOOST_FOREACH(Key key, keys) { - ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key)); + for(Key key: keys) { + ordering_.erase(find(ordering_.begin(), ordering_.end(), key)); delta_.erase(key); } } /* ************************************************************************* */ -void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { - - const bool debug = ISDEBUG("BatchFixedLagSmoother reorder"); - - if (debug) { - std::cout << "BatchFixedLagSmoother::reorder() START" << std::endl; - } - - if (debug) { - std::cout << "Marginalizable Keys: "; - BOOST_FOREACH(Key key, marginalizeKeys) { - std::cout << DefaultKeyFormatter(key) << " "; - } - std::cout << std::endl; - } - +void BatchFixedLagSmoother::reorder(const KeyVector& marginalizeKeys) { // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::ColamdConstrainedFirst(factors_, - std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); - - if (debug) { - ordering_.print("New Ordering: "); - } - - if (debug) { - std::cout << "BatchFixedLagSmoother::reorder() FINISH" << std::endl; - } + ordering_ = Ordering::ColamdConstrainedFirst(factors_, marginalizeKeys); } /* ************************************************************************* */ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { - const bool debug = ISDEBUG("BatchFixedLagSmoother optimize"); - - if (debug) { - std::cout << "BatchFixedLagSmoother::optimize() START" << std::endl; - } - // Create output result structure Result result; result.nonlinearVariables = theta_.size() - linearKeys_.size(); @@ -247,20 +199,9 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // check if we're already close enough if (result.error <= errorTol) { - if (debug) { - std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " - << result.error << " < " << errorTol << std::endl; - } return result; } - if (debug) { - std::cout << "BatchFixedLagSmoother::optimize linearValues: " - << linearKeys_.size() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize Initial error: " - << result.error << std::endl; - } - // Use a custom optimization loop so the linearization points can be controlled double previousError; VectorValues newDelta; @@ -276,19 +217,14 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Keep increasing lambda until we make make progress while (true) { - if (debug) { - std::cout << "BatchFixedLagSmoother::optimize trying lambda = " - << lambda << std::endl; - } - // Add prior factors at the current solution gttic(damp); GaussianFactorGraph dampedFactorGraph(linearFactorGraph); dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); { // for each of the variables, add a prior at the current solution - double sigma = 1.0 / std::sqrt(lambda); - BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) { + double sigma = 1.0 / sqrt(lambda); + for(const auto& key_value: delta_) { size_t dim = key_value.second.size(); Matrix A = Matrix::Identity(dim, dim); Vector b = key_value.second; @@ -314,13 +250,6 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double error = factors_.error(evalpoint); gttoc(compute_error); - if (debug) { - std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " - << newDelta.norm() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize next error = " << error - << std::endl; - } - if (error < result.error) { // Keep this change // Update the error value @@ -332,7 +261,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Put the linearization points and deltas back for specific variables if (enforceConsistency_ && (linearKeys_.size() > 0)) { theta_.update(linearKeys_); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) { + for(const auto& key_value: linearKeys_) { delta_.at(key_value.key) = newDelta.at(key_value.key); } } @@ -347,9 +276,9 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Reject this change if (lambda >= lambdaUpperBound) { // The maximum lambda has been used. Print a warning and end the search. - std::cout + cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" - << std::endl; + << endl; break; } else { // Increase lambda and continue searching @@ -360,87 +289,41 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } gttoc(optimizer_iteration); - if (debug) { - std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda - << std::endl; - } - result.iterations++; } while (result.iterations < maxIterations && !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); - if (debug) { - std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error - << std::endl; - } - - if (debug) { - std::cout << "BatchFixedLagSmoother::optimize() FINISH" << std::endl; - } - return result; } /* ************************************************************************* */ -void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { +void BatchFixedLagSmoother::marginalize(const KeyVector& marginalizeKeys) { // In order to marginalize out the selected variables, the factors involved in those variables // must be identified and removed. Also, the effect of those removed factors on the // remaining variables needs to be accounted for. This will be done with linear container factors // from the result of a partial elimination. This function removes the marginalized factors and // adds the linearized factors back in. - const bool debug = ISDEBUG("BatchFixedLagSmoother marginalize"); - - if (debug) - std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; - - if (debug) { - std::cout << "BatchFixedLagSmoother::marginalize Marginalize Keys: "; - BOOST_FOREACH(Key key, marginalizeKeys) { - std::cout << DefaultKeyFormatter(key) << " "; - } - std::cout << std::endl; - } - // Identify all of the factors involving any marginalized variable. These must be removed. - std::set removedFactorSlots; - VariableIndex variableIndex(factors_); - BOOST_FOREACH(Key key, marginalizeKeys) { + set removedFactorSlots; + const VariableIndex variableIndex(factors_); + for(Key key: marginalizeKeys) { const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(slots.begin(), slots.end()); } - if (debug) { - std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: "; - BOOST_FOREACH(size_t slot, removedFactorSlots) { - std::cout << slot << " "; - } - std::cout << std::endl; - } - // Add the removed factors to a factor graph NonlinearFactorGraph removedFactors; - BOOST_FOREACH(size_t slot, removedFactorSlots) { + for(size_t slot: removedFactorSlots) { if (factors_.at(slot)) { removedFactors.push_back(factors_.at(slot)); } } - if (debug) { - PrintSymbolicGraph(removedFactors, - "BatchFixedLagSmoother::marginalize Removed Factors: "); - } - // Calculate marginal factors on the remaining keys - NonlinearFactorGraph marginalFactors = calculateMarginalFactors( - removedFactors, theta_, marginalizeKeys, - parameters_.getEliminationFunction()); - - if (debug) { - PrintSymbolicGraph(removedFactors, - "BatchFixedLagSmoother::marginalize Marginal Factors: "); - } + NonlinearFactorGraph marginalFactors = CalculateMarginalFactors( + removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction()); // Remove marginalized factors from the factor graph removeFactors(removedFactorSlots); @@ -453,140 +336,96 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, - const std::string& label) { - std::cout << label; - BOOST_FOREACH(gtsam::Key key, keys) { - std::cout << " " << gtsam::DefaultKeyFormatter(key); +void BatchFixedLagSmoother::PrintKeySet(const set& keys, + const string& label) { + cout << label; + for(Key key: keys) { + cout << " " << DefaultKeyFormatter(key); } - std::cout << std::endl; + cout << endl; } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys, - const std::string& label) { - std::cout << label; - BOOST_FOREACH(gtsam::Key key, keys) { - std::cout << " " << gtsam::DefaultKeyFormatter(key); +void BatchFixedLagSmoother::PrintKeySet(const KeySet& keys, + const string& label) { + cout << label; + for(Key key: keys) { + cout << " " << DefaultKeyFormatter(key); } - std::cout << std::endl; + cout << endl; } /* ************************************************************************* */ void BatchFixedLagSmoother::PrintSymbolicFactor( const NonlinearFactor::shared_ptr& factor) { - std::cout << "f("; + cout << "f("; if (factor) { - BOOST_FOREACH(Key key, factor->keys()) { - std::cout << " " << gtsam::DefaultKeyFormatter(key); + for(Key key: factor->keys()) { + cout << " " << DefaultKeyFormatter(key); } } else { - std::cout << " NULL"; + cout << " NULL"; } - std::cout << " )" << std::endl; + cout << " )" << endl; } /* ************************************************************************* */ void BatchFixedLagSmoother::PrintSymbolicFactor( const GaussianFactor::shared_ptr& factor) { - std::cout << "f("; - BOOST_FOREACH(Key key, factor->keys()) { - std::cout << " " << gtsam::DefaultKeyFormatter(key); + cout << "f("; + for(Key key: factor->keys()) { + cout << " " << DefaultKeyFormatter(key); } - std::cout << " )" << std::endl; + cout << " )" << endl; } /* ************************************************************************* */ void BatchFixedLagSmoother::PrintSymbolicGraph( - const NonlinearFactorGraph& graph, const std::string& label) { - std::cout << label << std::endl; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { + const NonlinearFactorGraph& graph, const string& label) { + cout << label << endl; + for(const auto& factor: graph) { PrintSymbolicFactor(factor); } } /* ************************************************************************* */ void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, - const std::string& label) { - std::cout << label << std::endl; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { + const string& label) { + cout << label << endl; + for(const auto& factor: graph) { PrintSymbolicFactor(factor); } } /* ************************************************************************* */ -NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( - const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, +GaussianFactorGraph BatchFixedLagSmoother::CalculateMarginalFactors( + const GaussianFactorGraph& graph, const KeyVector& keys, const GaussianFactorGraph::Eliminate& eliminateFunction) { - - const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors"); - - if (debug) - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" - << std::endl; - - if (debug) - PrintKeySet(marginalizeKeys, - "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); - - // Get the set of all keys involved in the factor graph - KeySet allKeys(graph.keys()); - if (debug) - PrintKeySet(allKeys, - "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); - - if (!std::includes(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), - marginalizeKeys.end())) { - throw std::runtime_error("Some keys found in marginalizeKeys do not" - " occur in the graph."); - } - - // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys - KeySet remainingKeys; - std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), - marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); - if (debug) - PrintKeySet(remainingKeys, - "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); - - if (marginalizeKeys.size() == 0) { + if (keys.size() == 0) { // There are no keys to marginalize. Simply return the input factors - if (debug) - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" - << std::endl; return graph; } else { - - // Create the linear factor graph - GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); // .first is the eliminated Bayes tree, while .second is the remaining factor graph - GaussianFactorGraph marginalLinearFactors = - *linearFactorGraph.eliminatePartialMultifrontal( - std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; + return *graph.eliminatePartialMultifrontal(keys, eliminateFunction).second; + } +} + +/* ************************************************************************* */ +NonlinearFactorGraph BatchFixedLagSmoother::CalculateMarginalFactors( + const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys, + const GaussianFactorGraph::Eliminate& eliminateFunction) { + if (keys.size() == 0) { + // There are no keys to marginalize. Simply return the input factors + return graph; + } else { + // Create the linear factor graph + const auto linearFactorGraph = graph.linearize(theta); + + const auto marginalLinearFactors = + CalculateMarginalFactors(*linearFactorGraph, keys, eliminateFunction); // Wrap in nonlinear container factors - NonlinearFactorGraph marginalFactors; - marginalFactors.reserve(marginalLinearFactors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { - marginalFactors += boost::make_shared( - gaussianFactor, theta); - if (debug) { - std::cout - << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; - PrintSymbolicFactor(marginalFactors.back()); - } - } - - if (debug) - PrintSymbolicGraph(marginalFactors, - "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); - - if (debug) - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" - << std::endl; - - return marginalFactors; + return LinearContainerFactor::ConvertLinearGraph(marginalLinearFactors, theta); } } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 605f3a5e8..27fe94451 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -103,8 +103,26 @@ public: /// Calculate marginal covariance on given variable Matrix marginalCovariance(Key key) const; - static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + /// Marginalize specific keys from a linear graph. + /// Does not check whether keys actually exist in graph. + /// In that case will fail somewhere deep within elimination + static GaussianFactorGraph CalculateMarginalFactors( + const GaussianFactorGraph& graph, const KeyVector& keys, + const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky); + + /// Marginalize specific keys from a nonlinear graph, wrap in LinearContainers + static NonlinearFactorGraph CalculateMarginalFactors( + const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys, + const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky); + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + static NonlinearFactorGraph calculateMarginalFactors( + const NonlinearFactorGraph& graph, const Values& theta, const std::set& keys, + const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky) { + KeyVector keyVector(keys.begin(), keys.end()); + return CalculateMarginalFactors(graph, theta, keyVector, eliminateFunction); + } +#endif protected: @@ -147,16 +165,16 @@ protected: void removeFactors(const std::set& deleteFactors); /** Erase any keys associated with timestamps before the provided time */ - void eraseKeys(const std::set& keys); + void eraseKeys(const KeyVector& keys); /** Use colamd to update into an efficient ordering */ - void reorder(const std::set& marginalizeKeys = std::set()); + void reorder(const KeyVector& marginalizeKeys = KeyVector()); /** Optimize the current graph using a modified version of L-M */ Result optimize(); /** Marginalize out selected variables */ - void marginalize(const std::set& marginalizableKeys); + void marginalize(const KeyVector& marginalizableKeys); private: /** Private methods for printing debug information */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index c3e307010..884bba9a3 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -33,7 +33,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p } else { std::cout << "f( "; } - BOOST_FOREACH(Key key, *factor) { + for(Key key: *factor) { std::cout << keyFormatter(key) << " "; } std::cout << ")" << std::endl; @@ -46,7 +46,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { std::cout << indent << title << std::endl; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { + for(const NonlinearFactor::shared_ptr& factor: factors) { PrintNonlinearFactor(factor, indent + " ", keyFormatter); } } @@ -55,7 +55,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector& slots, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { std::cout << indent << title << std::endl; - BOOST_FOREACH(size_t slot, slots) { + for(size_t slot: slots) { PrintNonlinearFactor(factors.at(slot), indent + " ", keyFormatter); } } @@ -74,7 +74,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& } else { std::cout << "g( "; } - BOOST_FOREACH(Key key, *factor) { + for(Key key: *factor) { std::cout << keyFormatter(key) << " "; } std::cout << ")" << std::endl; @@ -87,7 +87,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& factors, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { std::cout << indent << title << std::endl; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + for(const GaussianFactor::shared_ptr& factor: factors) { PrintLinearFactor(factor, indent + " ", keyFormatter); } } @@ -139,7 +139,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto // Add the new variables to theta theta_.insert(newTheta); // Add new variables to the end of the ordering - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + for(const Values::ConstKeyValuePair& key_value: newTheta) { ordering_.push_back(key_value.key); } // Augment Delta @@ -222,7 +222,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm // Find the set of new separator keys KeySet newSeparatorKeys; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + for(const Values::ConstKeyValuePair& key_value: separatorValues_) { newSeparatorKeys.insert(key_value.key); } @@ -236,7 +236,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm graph.push_back(smootherShortcut_); Values values; values.insert(smootherSummarizationValues); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + for(const Values::ConstKeyValuePair& key_value: separatorValues_) { if(!values.exists(key_value.key)) { values.insert(key_value.key, key_value.value); } @@ -321,7 +321,7 @@ std::vector ConcurrentBatchFilter::insertFactors(const NonlinearFactorGr slots.reserve(factors.size()); // Insert the factor into an existing hole in the factor graph, if possible - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { + for(const NonlinearFactor::shared_ptr& factor: factors) { size_t slot; if(availableSlots_.size() > 0) { slot = availableSlots_.front(); @@ -345,7 +345,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { gttic(remove_factors); // For each factor slot to delete... - BOOST_FOREACH(size_t slot, slots) { + for(size_t slot: slots) { // Remove the factor from the graph factors_.remove(slot); @@ -431,7 +431,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values double sigma = 1.0 / std::sqrt(lambda); // for each of the variables, add a prior at the current solution - BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta) { + for(const VectorValues::KeyValuePair& key_value: delta) { size_t dim = key_value.second.size(); Matrix A = Matrix::Identity(dim,dim); Vector b = key_value.second; @@ -471,7 +471,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values // Put the linearization points and deltas back for specific variables if(linearValues.size() > 0) { theta.update(linearValues); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearValues) { + for(const Values::ConstKeyValuePair& key_value: linearValues) { delta.at(key_value.key) = newDelta.at(key_value.key); } } @@ -535,7 +535,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove) std::vector removedFactorSlots; VariableIndex variableIndex(factors_); - BOOST_FOREACH(Key key, keysToMove) { + for(Key key: keysToMove) { const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } @@ -544,14 +544,14 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { std::sort(removedFactorSlots.begin(), removedFactorSlots.end()); removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); // Remove any linear/marginal factor that made it into the set - BOOST_FOREACH(size_t index, separatorSummarizationSlots_) { + for(size_t index: separatorSummarizationSlots_) { removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end()); } // TODO: Make this compact if(debug) { std::cout << "ConcurrentBatchFilter::moveSeparator Removed Factor Slots: "; - BOOST_FOREACH(size_t slot, removedFactorSlots) { + for(size_t slot: removedFactorSlots) { std::cout << slot << " "; } std::cout << std::endl; @@ -559,7 +559,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { // Add these factors to a factor graph NonlinearFactorGraph removedFactors; - BOOST_FOREACH(size_t slot, removedFactorSlots) { + for(size_t slot: removedFactorSlots) { if(factors_.at(slot)) { removedFactors.push_back(factors_.at(slot)); } @@ -574,10 +574,10 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove KeySet newSeparatorKeys = removedFactors.keys(); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + for(const Values::ConstKeyValuePair& key_value: separatorValues_) { newSeparatorKeys.insert(key_value.key); } - BOOST_FOREACH(Key key, keysToMove) { + for(Key key: keysToMove) { newSeparatorKeys.erase(key); } @@ -585,7 +585,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys KeySet shortcutKeys = newSeparatorKeys; - BOOST_FOREACH(Key key, smootherSummarization_.keys()) { + for(Key key: smootherSummarization_.keys()) { shortcutKeys.insert(key); } @@ -632,7 +632,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { // Update the separatorValues object (should only contain the new separator keys) separatorValues_.clear(); - BOOST_FOREACH(Key key, separatorSummarization.keys()) { + for(Key key: separatorSummarization.keys()) { separatorValues_.insert(key, theta_.at(key)); } @@ -641,12 +641,12 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { removeFactors(removedFactorSlots); // Add the linearization point of the moved variables to the smoother cache - BOOST_FOREACH(Key key, keysToMove) { + for(Key key: keysToMove) { smootherValues_.insert(key, theta_.at(key)); } // Remove marginalized keys from values (and separator) - BOOST_FOREACH(Key key, keysToMove) { + for(Key key: keysToMove) { theta_.erase(key); ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key)); delta_.erase(key); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 9a458ee5a..53dd46fea 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -244,7 +244,7 @@ private: template void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { std::cout << indent << title; - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { std::cout << " " << keyFormatter(key); } std::cout << std::endl; diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 29ee14aee..d64b697b8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -28,7 +28,7 @@ namespace gtsam { void ConcurrentBatchSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s; std::cout << " Factors:" << std::endl; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { + for(const NonlinearFactor::shared_ptr& factor: factors_) { PrintNonlinearFactor(factor, " ", keyFormatter); } theta_.print("Values:\n"); @@ -61,7 +61,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF theta_.insert(newTheta); // Add new variables to the end of the ordering - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + for(const Values::ConstKeyValuePair& key_value: newTheta) { ordering_.push_back(key_value.key); } @@ -135,7 +135,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa removeFactors(filterSummarizationSlots_); // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues) { + for(const Values::ConstKeyValuePair& key_value: smootherValues) { std::pair iter_inserted = theta_.tryInsert(key_value.key, key_value.value); if(iter_inserted.second) { // If the insert succeeded @@ -146,7 +146,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa iter_inserted.first->value = key_value.value; } } - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues) { + for(const Values::ConstKeyValuePair& key_value: separatorValues) { std::pair iter_inserted = theta_.tryInsert(key_value.key, key_value.value); if(iter_inserted.second) { // If the insert succeeded @@ -188,7 +188,7 @@ std::vector ConcurrentBatchSmoother::insertFactors(const NonlinearFactor slots.reserve(factors.size()); // Insert the factor into an existing hole in the factor graph, if possible - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { + for(const NonlinearFactor::shared_ptr& factor: factors) { size_t slot; if(availableSlots_.size() > 0) { slot = availableSlots_.front(); @@ -212,7 +212,7 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector& slots) { gttic(remove_factors); // For each factor slot to delete... - BOOST_FOREACH(size_t slot, slots) { + for(size_t slot: slots) { // Remove the factor from the graph factors_.remove(slot); @@ -277,7 +277,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); { // for each of the variables, add a prior at the current solution - BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) { + for(const VectorValues::KeyValuePair& key_value: delta_) { size_t dim = key_value.second.size(); Matrix A = Matrix::Identity(dim,dim); Vector b = key_value.second; @@ -322,7 +322,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { // Put the linearization points and deltas back for specific variables if(separatorValues_.size() > 0) { theta_.update(separatorValues_); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + for(const Values::ConstKeyValuePair& key_value: separatorValues_) { delta_.at(key_value.key) = newDelta.at(key_value.key); } } @@ -366,13 +366,13 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { // Create a nonlinear factor graph without the filter summarization factors NonlinearFactorGraph graph(factors_); - BOOST_FOREACH(size_t slot, filterSummarizationSlots_) { + for(size_t slot: filterSummarizationSlots_) { graph.remove(slot); } // Get the set of separator keys gtsam::KeySet separatorKeys; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + for(const Values::ConstKeyValuePair& key_value: separatorValues_) { separatorKeys.insert(key_value.key); } @@ -389,7 +389,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared } else { std::cout << "f( "; } - BOOST_FOREACH(Key key, *factor) { + for(Key key: *factor) { std::cout << keyFormatter(key) << " "; } std::cout << ")" << std::endl; @@ -403,7 +403,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr std::cout << indent; if(factor) { std::cout << "g( "; - BOOST_FOREACH(Key key, *factor) { + for(Key key: *factor) { std::cout << keyFormatter(key) << " "; } std::cout << ")" << std::endl; diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index b893860cc..61f1c889f 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -77,7 +77,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, // Wrap in nonlinear container factors NonlinearFactorGraph marginalFactors; marginalFactors.reserve(marginalLinearFactors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { + for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) { marginalFactors += boost::make_shared(gaussianFactor, theta); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 1b7f86d5c..3913ba95c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -69,17 +69,17 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No int group = 1; // Set all existing variables to Group1 if(isam2_.getLinearizationPoint().size() > 0) { - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) { + for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) { orderingConstraints->operator[](key_value.key) = group; } ++group; } // Assign new variables to the root - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + for(const Values::ConstKeyValuePair& key_value: newTheta) { orderingConstraints->operator[](key_value.key) = group; } // Set marginalizable variables to Group0 - BOOST_FOREACH(Key key, *keysToMove){ + for(Key key: *keysToMove){ orderingConstraints->operator[](key) = 0; } } @@ -92,7 +92,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No boost::optional > additionalKeys = boost::none; if(keysToMove && keysToMove->size() > 0) { std::set markedKeys; - BOOST_FOREACH(Key key, *keysToMove) { + for(Key key: *keysToMove) { if(isam2_.getLinearizationPoint().exists(key)) { ISAM2Clique::shared_ptr clique = isam2_[key]; GaussianConditional::const_iterator key_iter = clique->conditional()->begin(); @@ -100,7 +100,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No markedKeys.insert(*key_iter); ++key_iter; } - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { RecursiveMarkAffectedKeys(key, child, markedKeys); } } @@ -120,7 +120,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No FactorIndices removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_); // Cache these factors for later transmission to the smoother NonlinearFactorGraph removedFactors; - BOOST_FOREACH(size_t slot, removedFactorSlots) { + for(size_t slot: removedFactorSlots) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { smootherFactors_.push_back(factor); @@ -128,7 +128,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No } } // Cache removed values for later transmission to the smoother - BOOST_FOREACH(Key key, *keysToMove) { + for(Key key: *keysToMove) { smootherValues_.insert(key, isam2_.getLinearizationPoint().at(key)); } gttoc(cache_smoother_factors); @@ -138,7 +138,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No FactorIndices deletedFactorsIndices; isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices); currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end()); - BOOST_FOREACH(size_t index, deletedFactorsIndices) { + for(size_t index: deletedFactorsIndices) { currentSmootherSummarizationSlots_.erase(std::remove(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end(), index), currentSmootherSummarizationSlots_.end()); } gttoc(marginalize); @@ -193,7 +193,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth graph.push_back(smootherShortcut_); Values values; values.insert(smootherSummarizationValues); - BOOST_FOREACH(Key key, newSeparatorKeys) { + for(Key key: newSeparatorKeys) { if(!values.exists(key)) { values.insert(key, isam2_.getLinearizationPoint().at(key)); } @@ -201,7 +201,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth // Force iSAM2 not to relinearize anything during this iteration FastList noRelinKeys; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) { + for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) { noRelinKeys.push_back(key_value.key); } @@ -232,7 +232,7 @@ void ConcurrentIncrementalFilter::getSummarizedFactors(NonlinearFactorGraph& fil filterSummarization = calculateFilterSummarization(); // Copy the current separator values into the output - BOOST_FOREACH(Key key, isam2_.getFixedVariables()) { + for(Key key: isam2_.getFixedVariables()) { filterSummarizationValues.insert(key, isam2_.getLinearizationPoint().at(key)); } @@ -271,12 +271,12 @@ void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, cons if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique - BOOST_FOREACH(Key idx, clique->conditional()->frontals()) { + for(Key idx: clique->conditional()->frontals()) { additionalKeys.insert(idx); } // Recursively mark all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { RecursiveMarkAffectedKeys(key, child, additionalKeys); } } @@ -290,7 +290,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove) FactorIndices removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } @@ -300,7 +300,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); // Remove any linear/marginal factor that made it into the set - BOOST_FOREACH(size_t index, factorsToIgnore) { + for(size_t index: factorsToIgnore) { removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end()); } @@ -313,13 +313,13 @@ void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& rem // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys KeySet shortcutKeys; - BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) { + for(size_t slot: currentSmootherSummarizationSlots_) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { shortcutKeys.insert(factor->begin(), factor->end()); } } - BOOST_FOREACH(Key key, previousSmootherSummarization_.keys()) { + for(Key key: previousSmootherSummarization_.keys()) { shortcutKeys.insert(key); } @@ -347,15 +347,15 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // Find all cliques that contain any separator variables std::set separatorCliques; - BOOST_FOREACH(Key key, separatorKeys) { + for(Key key: separatorKeys) { ISAM2Clique::shared_ptr clique = isam2_[key]; separatorCliques.insert( clique ); } // Create the set of clique keys LC: std::vector cliqueKeys; - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - BOOST_FOREACH(Key key, clique->conditional()->frontals()) { + for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { + for(Key key: clique->conditional()->frontals()) { cliqueKeys.push_back(key); } } @@ -363,8 +363,8 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // Gather all factors that involve only clique keys std::set cliqueFactorSlots; - BOOST_FOREACH(Key key, cliqueKeys) { - BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) { + for(Key key: cliqueKeys) { + for(size_t slot: isam2_.getVariableIndex()[key]) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { std::set factorKeys(factor->begin(), factor->end()); @@ -376,29 +376,29 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() } // Remove any factor included in the current smoother summarization - BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) { + for(size_t slot: currentSmootherSummarizationSlots_) { cliqueFactorSlots.erase(slot); } // Create a factor graph from the identified factors NonlinearFactorGraph graph; - BOOST_FOREACH(size_t slot, cliqueFactorSlots) { + for(size_t slot: cliqueFactorSlots) { graph.push_back(isam2_.getFactorsUnsafe().at(slot)); } // Find the set of children of the separator cliques std::set childCliques; // Add all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { + for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { childCliques.insert(clique->children.begin(), clique->children.end()); } // Remove any separator cliques that were added because they were children of other separator cliques - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { + for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { childCliques.erase(clique); } // Augment the factor graph with cached factors from the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) { + for(const ISAM2Clique::shared_ptr& clique: childCliques) { LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint())); graph.push_back( factor ); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 268160451..e95c1c81d 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -66,7 +66,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons // Also, mark the separator keys as fixed linearization points FastMap constrainedKeys; FastList noRelinKeys; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + for(const Values::ConstKeyValuePair& key_value: separatorValues_) { constrainedKeys[key_value.key] = 1; noRelinKeys.push_back(key_value.key); } @@ -82,12 +82,12 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons Values values(newTheta); // Unfortunately, we must be careful here, as some of the smoother values // and/or separator values may have been added previously - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { + for(const Values::ConstKeyValuePair& key_value: smootherValues_) { if(!isam2_.getLinearizationPoint().exists(key_value.key)) { values.insert(key_value.key, smootherValues_.at(key_value.key)); } } - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + for(const Values::ConstKeyValuePair& key_value: separatorValues_) { if(!isam2_.getLinearizationPoint().exists(key_value.key)) { values.insert(key_value.key, separatorValues_.at(key_value.key)); } @@ -188,15 +188,15 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { // Find all cliques that contain any separator variables std::set separatorCliques; - BOOST_FOREACH(Key key, separatorValues_.keys()) { + for(Key key: separatorValues_.keys()) { ISAM2Clique::shared_ptr clique = isam2_[key]; separatorCliques.insert( clique ); } // Create the set of clique keys LC: std::vector cliqueKeys; - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - BOOST_FOREACH(Key key, clique->conditional()->frontals()) { + for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { + for(Key key: clique->conditional()->frontals()) { cliqueKeys.push_back(key); } } @@ -204,8 +204,8 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { // Gather all factors that involve only clique keys std::set cliqueFactorSlots; - BOOST_FOREACH(Key key, cliqueKeys) { - BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) { + for(Key key: cliqueKeys) { + for(size_t slot: isam2_.getVariableIndex()[key]) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { std::set factorKeys(factor->begin(), factor->end()); @@ -217,36 +217,36 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { } // Remove any factor included in the filter summarization - BOOST_FOREACH(size_t slot, filterSummarizationSlots_) { + for(size_t slot: filterSummarizationSlots_) { cliqueFactorSlots.erase(slot); } // Create a factor graph from the identified factors NonlinearFactorGraph graph; - BOOST_FOREACH(size_t slot, cliqueFactorSlots) { + for(size_t slot: cliqueFactorSlots) { graph.push_back(isam2_.getFactorsUnsafe().at(slot)); } // Find the set of children of the separator cliques std::set childCliques; // Add all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { + for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { childCliques.insert(clique->children.begin(), clique->children.end()); } // Remove any separator cliques that were added because they were children of other separator cliques - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { + for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { childCliques.erase(clique); } // Augment the factor graph with cached factors from the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) { + for(const ISAM2Clique::shared_ptr& clique: childCliques) { LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint())); graph.push_back( factor ); } // Get the set of separator keys KeySet separatorKeys; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + for(const Values::ConstKeyValuePair& key_value: separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 369db51c3..62ee07a16 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -36,7 +36,7 @@ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { /* ************************************************************************* */ void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { // Loop through each key and add/update it in the map - BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { + for(const auto& key_timestamp: timestamps) { // Check to see if this key already exists in the database KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); @@ -64,8 +64,8 @@ void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) } /* ************************************************************************* */ -void FixedLagSmoother::eraseKeyTimestampMap(const std::set& keys) { - BOOST_FOREACH(Key key, keys) { +void FixedLagSmoother::eraseKeyTimestampMap(const KeyVector& keys) { + for(Key key: keys) { // Erase the key from the Timestamp->Key map double timestamp = keyTimestampMap_.at(key); @@ -92,21 +92,21 @@ double FixedLagSmoother::getCurrentTimestamp() const { } /* ************************************************************************* */ -std::set FixedLagSmoother::findKeysBefore(double timestamp) const { - std::set keys; +KeyVector FixedLagSmoother::findKeysBefore(double timestamp) const { + KeyVector keys; TimestampKeyMap::const_iterator end = timestampKeyMap_.lower_bound(timestamp); for(TimestampKeyMap::const_iterator iter = timestampKeyMap_.begin(); iter != end; ++iter) { - keys.insert(iter->second); + keys.push_back(iter->second); } return keys; } /* ************************************************************************* */ -std::set FixedLagSmoother::findKeysAfter(double timestamp) const { - std::set keys; +KeyVector FixedLagSmoother::findKeysAfter(double timestamp) const { + KeyVector keys; TimestampKeyMap::const_iterator begin = timestampKeyMap_.upper_bound(timestamp); for(TimestampKeyMap::const_iterator iter = begin; iter != timestampKeyMap_.end(); ++iter) { - keys.insert(iter->second); + keys.push_back(iter->second); } return keys; } diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index f212b38e7..4868a7cf1 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -24,7 +24,9 @@ #include #include #include + #include +#include namespace gtsam { @@ -111,16 +113,16 @@ protected: void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps); /** Erase keys from the Key-Timestamps database */ - void eraseKeyTimestampMap(const std::set& keys); + void eraseKeyTimestampMap(const KeyVector& keys); /** Find the most recent timestamp of the system */ double getCurrentTimestamp() const; /** Find all of the keys associated with timestamps before the provided time */ - std::set findKeysBefore(double timestamp) const; + KeyVector findKeysBefore(double timestamp) const; /** Find all of the keys associated with timestamps before the provided time */ - std::set findKeysAfter(double timestamp) const; + KeyVector findKeysAfter(double timestamp) const; }; // FixedLagSmoother diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 8623136cd..d3c34ff5e 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -34,12 +34,12 @@ void recursiveMarkAffectedKeys(const Key& key, != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique - BOOST_FOREACH(Key i, clique->conditional()->frontals()) { + for(Key i: clique->conditional()->frontals()) { additionalKeys.insert(i); } // Recursively mark all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { recursiveMarkAffectedKeys(key, child, additionalKeys); } } @@ -88,12 +88,12 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore( + KeyVector marginalizableKeys = findKeysBefore( current_timestamp - smootherLag_); if (debug) { std::cout << "Marginalizable Keys: "; - BOOST_FOREACH(Key key, marginalizableKeys) { + for(Key key: marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; } std::cout << std::endl; @@ -116,9 +116,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Mark additional keys between the marginalized keys and the leaves std::set additionalKeys; - BOOST_FOREACH(Key key, marginalizableKeys) { + for(Key key: marginalizableKeys) { ISAM2Clique::shared_ptr clique = isam_[key]; - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { recursiveMarkAffectedKeys(key, child, additionalKeys); } } @@ -174,17 +174,17 @@ void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) { /* ************************************************************************* */ void IncrementalFixedLagSmoother::createOrderingConstraints( - const std::set& marginalizableKeys, + const KeyVector& marginalizableKeys, boost::optional >& constrainedKeys) const { if (marginalizableKeys.size() > 0) { constrainedKeys = FastMap(); // Generate ordering constraints so that the marginalizable variables will be eliminated first // Set all variables to Group1 - BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { + for(const TimestampKeyMap::value_type& timestamp_key: timestampKeyMap_) { constrainedKeys->operator[](timestamp_key.second) = 1; } // Set marginalizable variables to Group0 - BOOST_FOREACH(Key key, marginalizableKeys) { + for(Key key: marginalizableKeys) { constrainedKeys->operator[](key) = 0; } } @@ -194,7 +194,7 @@ void IncrementalFixedLagSmoother::createOrderingConstraints( void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { std::cout << label; - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { std::cout << " " << DefaultKeyFormatter(key); } std::cout << std::endl; @@ -204,7 +204,7 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, void IncrementalFixedLagSmoother::PrintSymbolicFactor( const GaussianFactor::shared_ptr& factor) { std::cout << "f("; - BOOST_FOREACH(Key key, factor->keys()) { + for(Key key: factor->keys()) { std::cout << " " << DefaultKeyFormatter(key); } std::cout << " )" << std::endl; @@ -214,7 +214,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor( void IncrementalFixedLagSmoother::PrintSymbolicGraph( const GaussianFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { + for(const GaussianFactor::shared_ptr& factor: graph) { PrintSymbolicFactor(factor); } } @@ -224,7 +224,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTree(const ISAM2& isam, const std::string& label) { std::cout << label << std::endl; if (!isam.roots().empty()) { - BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) { + for(const ISAM2::sharedClique& root: isam.roots()) { PrintSymbolicTreeHelper(root); } } else @@ -237,18 +237,18 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper( // Print the current clique std::cout << indent << "P( "; - BOOST_FOREACH(Key key, clique->conditional()->frontals()) { + for(Key key: clique->conditional()->frontals()) { std::cout << DefaultKeyFormatter(key) << " "; } if (clique->conditional()->nrParents() > 0) std::cout << "| "; - BOOST_FOREACH(Key key, clique->conditional()->parents()) { + for(Key key: clique->conditional()->parents()) { std::cout << DefaultKeyFormatter(key) << " "; } std::cout << ")" << std::endl; // Recursively print all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { PrintSymbolicTreeHelper(child, indent + " "); } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 31ae20c50..77d052a21 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -117,7 +117,7 @@ protected: void eraseKeysBefore(double timestamp); /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ - void createOrderingConstraints(const std::set& marginalizableKeys, + void createOrderingConstraints(const KeyVector& marginalizableKeys, boost::optional >& constrainedKeys) const; private: diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 84dffe7be..e285955a7 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -16,7 +16,6 @@ */ #include -#include #include namespace gtsam { @@ -27,7 +26,7 @@ LinearizedGaussianFactor::LinearizedGaussianFactor( : NonlinearFactor(gaussian->keys()) { // Extract the keys and linearization points - BOOST_FOREACH(const Key& key, gaussian->keys()) { + for(const Key& key: gaussian->keys()) { // extract linearization point assert(lin_points.exists(key)); this->lin_points_.insert(key, lin_points.at(key)); @@ -65,7 +64,7 @@ void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& k std::cout << s << std::endl; std::cout << "Nonlinear Keys: "; - BOOST_FOREACH(const Key& key, this->keys()) + for(const Key& key: this->keys()) std::cout << keyFormatter(key) << " "; std::cout << std::endl; @@ -105,7 +104,7 @@ LinearizedJacobianFactor::linearize(const Values& c) const { // Create the 'terms' data structure for the Jacobian constructor std::vector > terms; - BOOST_FOREACH(Key key, keys()) { + for(Key key: keys()) { terms.push_back(std::make_pair(key, this->A(key))); } @@ -120,7 +119,7 @@ Vector LinearizedJacobianFactor::error_vector(const Values& c) const { Vector errorVector = -b(); - BOOST_FOREACH(Key key, this->keys()) { + for(Key key: this->keys()) { const Value& newPt = c.at(key); const Value& linPt = lin_points_.at(key); Vector d = linPt.localCoordinates_(newPt); @@ -140,21 +139,7 @@ LinearizedHessianFactor::LinearizedHessianFactor() { /* ************************************************************************* */ LinearizedHessianFactor::LinearizedHessianFactor( const HessianFactor::shared_ptr& hessian, const Values& lin_points) -: Base(hessian, lin_points) { - - // Create the dims array - size_t *dims = (size_t*)alloca(sizeof(size_t)*(hessian->size() + 1)); - size_t index = 0; - for(HessianFactor::const_iterator iter = hessian->begin(); iter != hessian->end(); ++iter) { - dims[index++] = hessian->getDim(iter); - } - dims[index] = 1; - - // Update the BlockInfo accessor - info_ = SymmetricBlockMatrix(dims, dims+hessian->size()+1); - // Copy the augmented matrix holding G, g, and f - info_.full() = hessian->info(); -} + : Base(hessian, lin_points), info_(hessian->info()) {} /* ************************************************************************* */ void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { @@ -162,11 +147,11 @@ void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& ke std::cout << s << std::endl; std::cout << "Nonlinear Keys: "; - BOOST_FOREACH(const Key& key, this->keys()) + for(const Key& key: this->keys()) std::cout << keyFormatter(key) << " "; std::cout << std::endl; - gtsam::print(Matrix(info_.full()), "Ab^T * Ab: "); + gtsam::print(Matrix(info_.selfadjointView()), "Ab^T * Ab: "); lin_points_.print("Linearization Point: "); } @@ -177,9 +162,9 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol const This *e = dynamic_cast (&expected); if (e) { - Matrix thisMatrix = this->info_.full(); + Matrix thisMatrix = this->info_.selfadjointView(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = e->info_.full(); + Matrix rhsMatrix = e->info_.selfadjointView(); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; return Base::equals(expected, tol) @@ -235,16 +220,20 @@ LinearizedHessianFactor::linearize(const Values& c) const { //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView() * dx; Vector g = linearTerm() - squaredTerm() * dx; std::vector gs; + std::size_t offset = 0; for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { - gs.push_back(g.segment(info_.offset(i), info_.offset(i+1) - info_.offset(i))); + const std::size_t dim = info_.getDim(i); + gs.push_back(g.segment(offset, dim)); + offset += dim; } // G2 = G1 // Do Nothing std::vector Gs; for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { - for(DenseIndex j = i; j < info_.nBlocks()-1; ++j) { - Gs.push_back(info_(i,j)); + Gs.push_back(info_.diagonalBlock(i)); + for(DenseIndex j = i + 1; j < info_.nBlocks()-1; ++j) { + Gs.push_back(info_.aboveDiagonalBlock(i, j)); } } diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index c6710bd70..f9fa6033f 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -178,8 +178,9 @@ public: /** hessian block data types */ typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version) - typedef SymmetricBlockMatrix::Block::OffDiagonal::ColXpr Column; ///< A column containing the linear term h - typedef SymmetricBlockMatrix::constBlock::OffDiagonal::ColXpr constColumn; ///< A column containing the linear term h (const version) + + typedef SymmetricBlockMatrix::Block::ColXpr Column; ///< A column containing the linear term h + typedef SymmetricBlockMatrix::constBlock::ColXpr constColumn; ///< A column containing the linear term h (const version) protected: @@ -216,19 +217,26 @@ public: /** Return the constant term \f$ f \f$ as described above * @return The constant term \f$ f \f$ */ - double constantTerm() const { return info_(this->size(), this->size())(0,0); }; + double constantTerm() const { + const auto block = info_.diagonalBlock(size()); + return block(0, 0); + } /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ - constColumn linearTerm(const_iterator j) const { return info_(j - this->begin(), this->size()).knownOffDiagonal().col(0); } + constColumn linearTerm(const_iterator j) const { + return info_.aboveDiagonalRange(j - begin(), size(), size(), size() + 1).col(0); + } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - constColumn linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); }; + constColumn linearTerm() const { + return info_.aboveDiagonalRange(0, size(), size(), size() + 1).col(0); + } - /** Return a view of the block at (j1,j2) of the upper-triangular part of the + /** Return a copy of the block at (j1,j2) of the upper-triangular part of the * squared term \f$ H \f$, no data is copied. See HessianFactor class documentation * above to explain that only the upper-triangular part of the information matrix is stored * and returned by this function. @@ -236,19 +244,24 @@ public: * use, for example, begin() + 2 to get the 3rd variable in this factor. * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return A view of the requested block, not a copy. + * @return A copy of the requested block. */ - constBlock squaredTerm(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } + Matrix squaredTerm(const_iterator j1, const_iterator j2) const { + const DenseIndex J1 = j1 - begin(); + const DenseIndex J2 = j2 - begin(); + return info_.block(J1, J2); + } /** Return the upper-triangular part of the full squared term, as described above. * See HessianFactor class documentation above to explain that only the * upper-triangular part of the information matrix is stored and returned by this function. */ - constBlock::SelfAdjointView squaredTerm() const { return info_.range(0, this->size(), 0, this->size()).selfadjointView(); } - + Eigen::SelfAdjointView squaredTerm() const { + return info_.selfadjointView(0, size()); + } /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const { return info_.rows() - 1; }; + size_t dim() const { return info_.rows() - 1; } /** Calculate the error of the factor */ double error(const Values& c) const; diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h new file mode 100644 index 000000000..b1f463c26 --- /dev/null +++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h @@ -0,0 +1,101 @@ +/** + * @file NonlinearClusterTree.h + * @author Frank Dellaert + * @date March, 2016 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { +class NonlinearClusterTree : public ClusterTree { + public: + NonlinearClusterTree() {} + + struct NonlinearCluster : Cluster { + // Given graph, index, add factors with specified keys into + // Factors are erased in the graph + // TODO(frank): fairly hacky and inefficient. Think about iterating the graph once instead + NonlinearCluster(const VariableIndex& variableIndex, const std::vector& keys, + NonlinearFactorGraph* graph) { + for (const Key key : keys) { + std::vector factors; + for (auto i : variableIndex[key]) + if (graph->at(i)) { + factors.push_back(graph->at(i)); + graph->remove(i); + } + Cluster::addFactors(key, factors); + } + } + + GaussianFactorGraph::shared_ptr linearize(const Values& values) { + return factors.linearize(values); + } + + static NonlinearCluster* DownCast(const boost::shared_ptr& cluster) { + auto nonlinearCluster = boost::dynamic_pointer_cast(cluster); + if (!nonlinearCluster) + throw std::runtime_error("Expected NonlinearCluster"); + return nonlinearCluster.get(); + } + + // linearize local custer factors straight into hessianFactor, which is returned + // If no ordering given, uses colamd + HessianFactor::shared_ptr linearizeToHessianFactor( + const Values& values, boost::optional ordering = boost::none, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + if (!ordering) + ordering.reset(Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true)); + return factors.linearizeToHessianFactor(values, *ordering, dampen); + } + + // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // TODO(frank): Use TBB to support deep trees and parallelism + std::pair linearizeAndEliminate( + const Values& values, boost::optional ordering = boost::none, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + // Linearize and create HessianFactor f(front,separator) + HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen); + + // Get contributions f(front) from children, as well as p(children|front) + GaussianBayesNet bayesNet; + for (const auto& child : children) { + auto message = DownCast(child)->linearizeAndEliminate(values, &bayesNet); + message->updateHessian(localFactor.get()); + } + auto gaussianConditional = localFactor->eliminateCholesky(orderedFrontalKeys); + bayesNet.add(gaussianConditional); + return {bayesNet, localFactor}; + } + + // Recursively eliminate subtree rooted at this Cluster + // Version that updates existing Bayes net and returns a new Hessian factor on parent clique + // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor + HessianFactor::shared_ptr linearizeAndEliminate( + const Values& values, GaussianBayesNet* bayesNet, + boost::optional ordering = boost::none, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + auto bayesNet_newFactor_pair = linearizeAndEliminate(values, ordering, dampen); + if (bayesNet) { + bayesNet->push_back(bayesNet_newFactor_pair.first); + } + return bayesNet_newFactor_pair.second; + } + }; + + // Linearize and update linearization point with values + Values updateCholesky(const Values& values) { + GaussianBayesNet bayesNet; + for (const auto& root : roots_) { + auto result = NonlinearCluster::DownCast(root)->linearizeAndEliminate(values); + bayesNet.push_back(result.first); + } + VectorValues delta = bayesNet.optimize(); + return values.retract(delta); + } +}; +} // namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index d359d0eff..4aff1b465 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -64,18 +64,18 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, std::set KeysToKeep; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linPoint) { // we cycle over all the keys of factorGraph + for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph KeysToKeep.insert(key_value.key); } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize - BOOST_FOREACH(Key key, keysToMarginalize) { + for(Key key: keysToMarginalize) { KeysToKeep.erase(key); } // we removed the keys that we have to marginalize Ordering ordering; - BOOST_FOREACH(Key key, keysToMarginalize) { + for(Key key: keysToMarginalize) { ordering.push_back(key); } // the keys that we marginalize should be at the beginning in the ordering - BOOST_FOREACH(Key key, KeysToKeep) { + for(Key key: KeysToKeep) { ordering.push_back(key); } @@ -84,7 +84,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second; NonlinearFactorGraph LinearContainerForGaussianMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + for(const GaussianFactor::shared_ptr& factor: marginal) { LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint)); } return LinearContainerForGaussianMarginals; @@ -403,9 +403,9 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); Values partialValues; partialValues.insert(1, optimalValues.at(1)); @@ -437,10 +437,10 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { - expectedGraph.push_back(LinearContainerFactor(factor, partialValues)); + for(const GaussianFactor::shared_ptr& factor: result) { + expectedGraph.emplace_shared(factor, partialValues); } @@ -451,7 +451,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) Values expectedValues = optimalValues; // Check - BOOST_FOREACH(Key key, keysToMove) { + for(Key key: keysToMove) { expectedValues.erase(key); } @@ -1014,7 +1014,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 ) GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second; NonlinearFactorGraph expectedMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { + for(const GaussianFactor::shared_ptr& factor: result) { expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); } @@ -1069,7 +1069,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 ) GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second; NonlinearFactorGraph expectedMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { + for(const GaussianFactor::shared_ptr& factor: result) { expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); } @@ -1126,13 +1126,13 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1181,11 +1181,11 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -1238,10 +1238,10 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1258,11 +1258,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -1291,11 +1291,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 558654367..a0b6e2c1b 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -560,14 +560,14 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); KeySet eliminateKeys = linearFactors->keys(); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { + for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { eliminateKeys.erase(key_value.key); } std::vector variables(eliminateKeys.begin(), eliminateKeys.end()); GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second; expectedSmootherSummarization.resize(0); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { + for(const GaussianFactor::shared_ptr& factor: result) { expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); } @@ -617,13 +617,13 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -670,11 +670,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -724,10 +724,10 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -744,11 +744,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // Add some factors to the Smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -774,11 +774,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index a283ece29..d6b7ab851 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -62,7 +62,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int // it is the same as the input graph, but we removed the empty factors that may be present in the input graph NonlinearFactorGraph graphForISAM2; - BOOST_FOREACH(NonlinearFactor::shared_ptr factor, graph){ + for(NonlinearFactor::shared_ptr factor: graph){ if(factor) graphForISAM2.push_back(factor); } @@ -80,18 +80,18 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, std::set KeysToKeep; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linPoint) { // we cycle over all the keys of factorGraph + for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph KeysToKeep.insert(key_value.key); } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize - BOOST_FOREACH(Key key, keysToMarginalize) { + for(Key key: keysToMarginalize) { KeysToKeep.erase(key); } // we removed the keys that we have to marginalize Ordering ordering; - BOOST_FOREACH(Key key, keysToMarginalize) { + for(Key key: keysToMarginalize) { ordering.push_back(key); } // the keys that we marginalize should be at the beginning in the ordering - BOOST_FOREACH(Key key, KeysToKeep) { + for(Key key: KeysToKeep) { ordering.push_back(key); } @@ -101,7 +101,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second; NonlinearFactorGraph LinearContainerForGaussianMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + for(const GaussianFactor::shared_ptr& factor: marginal) { LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint)); } @@ -417,15 +417,15 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) Values expectedValues = optimalValues; // Check - BOOST_FOREACH(Key key, keysToMove) { + for(Key key: keysToMove) { expectedValues.erase(key); } // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues); @@ -441,9 +441,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + for(const GaussianFactor::shared_ptr& factor: marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, // therefore if we add it here it will not pass the test // expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues)); @@ -501,15 +501,15 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) Values expectedValues = optimalValues; // Check - BOOST_FOREACH(Key key, keysToMove) { + for(Key key: keysToMove) { expectedValues.erase(key); } // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues); @@ -525,9 +525,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + for(const GaussianFactor::shared_ptr& factor: marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, // therefore if we add it here it will not pass the test // expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues)); @@ -543,7 +543,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) Values optimalValues2 = BatchOptimize(newFactors, optimalValues); Values expectedValues2 = optimalValues2; // Check - BOOST_FOREACH(Key key, keysToMove) { + for(Key key: keysToMove) { expectedValues2.erase(key); } Values actualValues2 = filter.calculateEstimate(); @@ -1116,7 +1116,7 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 ) GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second; NonlinearFactorGraph expectedMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + for(const GaussianFactor::shared_ptr& factor: marginal) { expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); } @@ -1167,7 +1167,7 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second; NonlinearFactorGraph expectedMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + for(const GaussianFactor::shared_ptr& factor: marginal) { expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); } @@ -1231,13 +1231,13 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)) // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1289,11 +1289,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -1350,10 +1350,10 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1406,11 +1406,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) Values actualValues = filter.getLinearizationPoint(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 22dd0ccaa..c265bf5d1 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -512,7 +512,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 ) // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1); Values expectedLinearizationPoint = filterSeparatorValues; Values actualLinearizationPoint; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { + for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key)); } CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6)); @@ -580,14 +580,14 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); KeySet allkeys = LinFactorGraph->keys(); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { + for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { allkeys.erase(key_value.key); } std::vector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); expectedSmootherSummarization.resize(0); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *result.second) { + for(const GaussianFactor::shared_ptr& factor: *result.second) { expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 858fbb75c..fdf9f08b5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 ) // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1); Values expectedLinearizationPoint = filterSeparatorValues; Values actualLinearizationPoint; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { + for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key)); } CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6)); @@ -582,13 +582,13 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); KeySet allkeys = LinFactorGraph->keys(); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) + for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) allkeys.erase(key_value.key); std::vector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); expectedSmootherSummarization.resize(0); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *result.second) { + for(const GaussianFactor::shared_ptr& factor: *result.second) { expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); } @@ -639,13 +639,13 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) actualGraph.print("actual graph: \n"); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp new file mode 100644 index 000000000..2240034af --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -0,0 +1,155 @@ +/** + * @file testNonlinearClusterTree.cpp + * @author Frank Dellaert + * @date March, 2016 + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include + +using namespace gtsam; + +static const Symbol x1('x', 1), x2('x', 2), x3('x', 3); +static const Symbol l1('l', 1), l2('l', 2); + +/* ************************************************************************* */ +NonlinearFactorGraph planarSLAMGraph() { + NonlinearFactorGraph graph; + + // Prior on pose x1 at the origin. + Pose2 prior(0.0, 0.0, 0.0); + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + graph.emplace_shared >(x1, prior, priorNoise); + + // Two odometry factors + Pose2 odometry(2.0, 0.0, 0.0); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(x2, x3, odometry, odometryNoise); + + // Add Range-Bearing measurements to two different landmarks + auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); + Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), + bearing32 = Rot2::fromDegrees(90); + double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0; + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); + + return graph; +} + +/* ************************************************************************* */ +// Create initial estimate +Values planarSLAMValues() { + Values initial; + initial.insert(l1, Point2(1.8, 2.1)); + initial.insert(l2, Point2(4.1, 1.8)); + initial.insert(x1, Pose2(0.5, 0.0, 0.2)); + initial.insert(x2, Pose2(2.3, 0.1, -0.2)); + initial.insert(x3, Pose2(4.1, 0.1, 0.1)); + return initial; +} + +/* ************************************************************************* */ +TEST(NonlinearClusterTree, Clusters) { + NonlinearFactorGraph graph = planarSLAMGraph(); + Values initial = planarSLAMValues(); + + // Build the clusters + // NOTE(frank): Order matters here as factors are removed! + VariableIndex variableIndex(graph); + typedef NonlinearClusterTree::NonlinearCluster Cluster; + auto marginalCluster = boost::shared_ptr(new Cluster(variableIndex, {x1}, &graph)); + auto landmarkCluster = boost::shared_ptr(new Cluster(variableIndex, {l1, l2}, &graph)); + auto rootCluster = boost::shared_ptr(new Cluster(variableIndex, {x2, x3}, &graph)); + + EXPECT_LONGS_EQUAL(3, marginalCluster->nrFactors()); + EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFactors()); + EXPECT_LONGS_EQUAL(1, rootCluster->nrFactors()); + + EXPECT_LONGS_EQUAL(1, marginalCluster->nrFrontals()); + EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFrontals()); + EXPECT_LONGS_EQUAL(2, rootCluster->nrFrontals()); + + // Test linearize + auto gfg = marginalCluster->linearize(initial); + EXPECT_LONGS_EQUAL(3, gfg->size()); + + // Calculate expected result of only evaluating the marginalCluster + Ordering ordering; + ordering.push_back(x1); + auto expected = gfg->eliminatePartialSequential(ordering); + auto expectedFactor = boost::dynamic_pointer_cast(expected.second->at(0)); + if (!expectedFactor) + throw std::runtime_error("Expected HessianFactor"); + + // Linearize and eliminate the marginalCluster + auto actual = marginalCluster->linearizeAndEliminate(initial); + const GaussianBayesNet& bayesNet = actual.first; + const HessianFactor& factor = *actual.second; + EXPECT(assert_equal(*expected.first->at(0), *bayesNet.at(0), 1e-6)); + EXPECT(assert_equal(*expectedFactor, factor, 1e-6)); +} + +/* ************************************************************************* */ +static NonlinearClusterTree Construct() { + // Build the clusters + // NOTE(frank): Order matters here as factors are removed! + NonlinearFactorGraph graph = planarSLAMGraph(); + VariableIndex variableIndex(graph); + typedef NonlinearClusterTree::NonlinearCluster Cluster; + auto marginalCluster = boost::shared_ptr(new Cluster(variableIndex, {x1}, &graph)); + auto landmarkCluster = boost::shared_ptr(new Cluster(variableIndex, {l1, l2}, &graph)); + auto rootCluster = boost::shared_ptr(new Cluster(variableIndex, {x2, x3}, &graph)); + + // Build the tree + NonlinearClusterTree clusterTree; + clusterTree.addRoot(rootCluster); + rootCluster->addChild(landmarkCluster); + landmarkCluster->addChild(marginalCluster); + + return clusterTree; +} + +/* ************************************************************************* */ +TEST(NonlinearClusterTree, Construct) { + NonlinearClusterTree clusterTree = Construct(); + + EXPECT_LONGS_EQUAL(3, clusterTree[0].problemSize()); + EXPECT_LONGS_EQUAL(3, clusterTree[0][0].problemSize()); + EXPECT_LONGS_EQUAL(3, clusterTree[0][0][0].problemSize()); +} + +/* ************************************************************************* */ +TEST(NonlinearClusterTree, Solve) { + NonlinearClusterTree clusterTree = Construct(); + + Values expected; + expected.insert(l1, Point2(2, 2)); + expected.insert(l2, Point2(4, 2)); + expected.insert(x1, Pose2(0, 0, 0)); + expected.insert(x2, Pose2(2, 0, 0)); + expected.insert(x3, Pose2(4, 0, 0)); + + Values values = planarSLAMValues(); + for (size_t i = 0; i < 4; i++) + values = clusterTree.updateCholesky(values); + + EXPECT(assert_equal(expected, values, 1e-7)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index ace13dc41..a65e3f1ca 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -13,7 +13,6 @@ #include #include #include -#include #include #include @@ -193,7 +192,7 @@ namespace gtsam { namespace partition { std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; int index1, index2; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + for(const typename GenericGraph::value_type& factor: graph){ index1 = dictionary[factor->key1.index]; index2 = dictionary[factor->key2.index]; std::cout << "index1: " << index1 << std::endl; @@ -222,7 +221,7 @@ namespace gtsam { namespace partition { sharedInts& adjncy = *ptr_adjncy; sharedInts& adjwgt = *ptr_adjwgt; int ind_xadj = 0, ind_adjncy = 0; - BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) { + for(const NeighborsInfo& info: adjacencyMap) { *(xadj.get() + ind_xadj) = ind_adjncy; std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy); std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy); @@ -334,7 +333,7 @@ namespace gtsam { namespace partition { << " " << result.B.size() << std::endl; int edgeCut = 0; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + for(const typename GenericGraph::value_type& factor: graph){ int key1 = factor->key1.index; int key2 = factor->key2.index; // print keys and their subgraph assignment @@ -372,19 +371,19 @@ namespace gtsam { namespace partition { // debug functions void printIsland(const std::vector& island) { std::cout << "island: "; - BOOST_FOREACH(const size_t key, island) + for(const size_t key: island) std::cout << key << " "; std::cout << std::endl; } void printIslands(const std::list >& islands) { - BOOST_FOREACH(const std::vector& island, islands) + for(const std::vector& island: islands) printIsland(island); } void printNumCamerasLandmarks(const std::vector& keys, const std::vector& int2symbol) { int numCamera = 0, numLandmark = 0; - BOOST_FOREACH(const size_t key, keys) + for(const size_t key: keys) if (int2symbol[key].chr() == 'x') numCamera++; else @@ -403,16 +402,16 @@ namespace gtsam { namespace partition { std::vector& C = partitionResult.C; std::vector& dictionary = workspace.dictionary; std::fill(dictionary.begin(), dictionary.end(), -1); - BOOST_FOREACH(const size_t a, A) + for(const size_t a: A) dictionary[a] = 1; - BOOST_FOREACH(const size_t b, B) + for(const size_t b: B) dictionary[b] = 2; if (!C.empty()) throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); // set up landmarks size_t i,j; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { + for(const typename GenericGraph::value_type& factor: graph) { i = factor->key1.index; j = factor->key2.index; if (dictionary[j] == 0) // if the landmark is already in the separator, continue @@ -427,7 +426,7 @@ namespace gtsam { namespace partition { // std::cout << "dictionary[67980]" << dictionary[j] << std::endl; } - BOOST_FOREACH(const size_t j, landmarkKeys) { + for(const size_t j: landmarkKeys) { switch(dictionary[j]) { case 0: C.push_back(j); break; case 1: A.push_back(j); break; @@ -456,7 +455,7 @@ namespace gtsam { namespace partition { // find out all the landmark keys, which are to be eliminated cameraKeys.reserve(keys.size()); landmarkKeys.reserve(keys.size()); - BOOST_FOREACH(const size_t key, keys) { + for(const size_t key: keys) { if((*int2symbol)[key].chr() == 'x') cameraKeys.push_back(key); else @@ -518,11 +517,11 @@ namespace gtsam { namespace partition { // printNumCamerasLandmarks(result->C, *int2symbol); // std::cout << "no. of island: " << islands.size() << "; "; // std::cout << "island size: "; -// BOOST_FOREACH(const Island& island, islands) +// for(const Island& island: islands) // std::cout << island.size() << " "; // std::cout << std::endl; -// BOOST_FOREACH(const Island& island, islands) { +// for(const Island& island: islands) { // printNumCamerasLandmarks(island, int2symbol); // } #endif @@ -550,12 +549,12 @@ namespace gtsam { namespace partition { // generate the node map std::vector& partitionTable = workspace.partitionTable; std::fill(partitionTable.begin(), partitionTable.end(), -1); - BOOST_FOREACH(const size_t key, result->C) + for(const size_t key: result->C) partitionTable[key] = 0; int idx = 0; - BOOST_FOREACH(const Island& island, islands) { + for(const Island& island: islands) { idx++; - BOOST_FOREACH(const size_t key, island) { + for(const size_t key: island) { partitionTable[key] = idx; } } diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index b78c1d3dc..3ac77c213 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -6,7 +6,6 @@ * Description: generic graph types used in partitioning */ #include -#include #include #include #include @@ -98,7 +97,7 @@ namespace gtsam { namespace partition { } // erase unused factors - BOOST_FOREACH(const FactorList::iterator& it, toErase) + for(const FactorList::iterator& it: toErase) factors.erase(it); if (!succeed) break; @@ -107,7 +106,7 @@ namespace gtsam { namespace partition { list > islands; map > arrays = dsf.arrays(); size_t key; vector array; - BOOST_FOREACH(boost::tie(key, array), arrays) + for(boost::tie(key, array): arrays) islands.push_back(array); return islands; } @@ -116,14 +115,14 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ void print(const GenericGraph2D& graph, const std::string name) { cout << name << endl; - BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) + for(const sharedGenericFactor2D& factor_: graph) cout << factor_->key1.index << " " << factor_->key2.index << endl; } /* ************************************************************************* */ void print(const GenericGraph3D& graph, const std::string name) { cout << name << endl; - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) + for(const sharedGenericFactor3D& factor_: graph) cout << factor_->key1.index << " " << factor_->key2.index << " (" << factor_->key1.type << ", " << factor_->key2.type <<")" << endl; } @@ -174,7 +173,7 @@ namespace gtsam { namespace partition { } // erase unused factors - BOOST_FOREACH(const FactorList::iterator& it, toErase) + for(const FactorList::iterator& it: toErase) factors.erase(it); if (!succeed) break; @@ -204,7 +203,7 @@ namespace gtsam { namespace partition { // compute the constraint number per camera std::fill(nrConstraints.begin(), nrConstraints.end(), 0); - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + for(const sharedGenericFactor3D& factor_: graph) { const int& key1 = factor_->key1.index; const int& key2 = factor_->key2.index; if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && @@ -258,7 +257,7 @@ namespace gtsam { namespace partition { // check the constraint number of every variable // find the camera and landmark keys - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + for(const sharedGenericFactor3D& factor_: graph) { //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM if (workspace.dictionary[factor_->key1.index] != -1) { if (factor_->key1.type == NODE_POSE_3D) @@ -287,7 +286,7 @@ namespace gtsam { namespace partition { // add singular variables directly as islands if (!singularCameras.empty()) { if (verbose) cout << "singular cameras:"; - BOOST_FOREACH(const size_t i, singularCameras) { + for(const size_t i: singularCameras) { islands.push_back(vector(1, i)); // <--------------------------- if (verbose) cout << i << " "; } @@ -295,7 +294,7 @@ namespace gtsam { namespace partition { } if (!singularLandmarks.empty()) { if (verbose) cout << "singular landmarks:"; - BOOST_FOREACH(const size_t i, singularLandmarks) { + for(const size_t i: singularLandmarks) { islands.push_back(vector(1, i)); // <--------------------------- if (verbose) cout << i << " "; } @@ -306,10 +305,10 @@ namespace gtsam { namespace partition { // regenerating islands map > labelIslands = dsf.arrays(); size_t label; vector island; - BOOST_FOREACH(boost::tie(label, island), labelIslands) { + for(boost::tie(label, island): labelIslands) { vector filteredIsland; // remove singular cameras from array filteredIsland.reserve(island.size()); - BOOST_FOREACH(const size_t key, island) { + for(const size_t key: island) { if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined @@ -321,7 +320,7 @@ namespace gtsam { namespace partition { // sanity check size_t nrKeys = 0; - BOOST_FOREACH(const vector& island, islands) + for(const vector& island: islands) nrKeys += island.size(); if (nrKeys != keys.size()) { cout << nrKeys << " vs " << keys.size() << endl; @@ -363,7 +362,7 @@ namespace gtsam { namespace partition { // for odometry xi-xj where i cameraToCamera(dictionary.size(), -1); size_t key_i, key_j; - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + for(const sharedGenericFactor3D& factor_: graph) { if (factor_->key1.type == NODE_POSE_3D) { if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); @@ -382,7 +381,7 @@ namespace gtsam { namespace partition { } // sort the landmark keys for the late getNrCommonLandmarks call - BOOST_FOREACH(vector &landmarks, cameraToLandmarks){ + for(vector &landmarks: cameraToLandmarks){ if (!landmarks.empty()) std::sort(landmarks.begin(), landmarks.end()); } @@ -417,7 +416,7 @@ namespace gtsam { namespace partition { const vector& dictionary = workspace.dictionary; vector isValidCamera(workspace.dictionary.size(), false); vector isValidLandmark(workspace.dictionary.size(), false); - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + for(const sharedGenericFactor3D& factor_: graph) { assert(factor_->key1.type == NODE_POSE_3D); //assert(factor_->key2.type == NODE_LANDMARK_3D); const size_t& key1 = factor_->key1.index; @@ -463,7 +462,7 @@ namespace gtsam { namespace partition { } // debug info - BOOST_FOREACH(const size_t key, frontals) { + for(const size_t key: frontals) { if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera) cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl; } diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h index 6f1818a3e..28076bda0 100644 --- a/gtsam_unstable/partition/NestedDissection-inl.h +++ b/gtsam_unstable/partition/NestedDissection-inl.h @@ -70,11 +70,11 @@ namespace gtsam { namespace partition { boost::shared_ptr NestedDissection::makeSubNLG( const NLG& fg, const vector& frontals, const vector& sep, const boost::shared_ptr& parent) const { OrderedSymbols frontalKeys; - BOOST_FOREACH(const size_t index, frontals) + for(const size_t index: frontals) frontalKeys.push_back(int2symbol_[index]); UnorderedSymbols sepKeys; - BOOST_FOREACH(const size_t index, sep) + for(const size_t index: sep) sepKeys.insert(int2symbol_[index]); return boost::make_shared(fg, frontalKeys, sepKeys, parent); @@ -129,7 +129,7 @@ namespace gtsam { namespace partition { // make three lists of variables A, B, and C int partition; childFrontals.resize(numSubmaps); - BOOST_FOREACH(const size_t key, keys){ + for(const size_t key: keys){ partition = partitionTable[key]; switch (partition) { case -1: break; // the separator of the separator variables @@ -144,13 +144,13 @@ namespace gtsam { namespace partition { childSeps.reserve(numSubmaps); frontalFactors.resize(numSubmaps); frontalUnaryFactors.resize(numSubmaps); - BOOST_FOREACH(typename GenericGraph::value_type factor, fg) + for(typename GenericGraph::value_type factor: fg) processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); - BOOST_FOREACH(const set& childSep, childSeps_) + for(const set& childSep: childSeps_) childSeps.push_back(vector(childSep.begin(), childSep.end())); // add unary factor to the current cluster or pass it to one of the child clusters - BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { + for(const sharedGenericUnaryFactor& unaryFactor_: unaryFactors) { partition = partitionTable[unaryFactor_->key.index]; if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); else frontalUnaryFactors[partition-1].push_back(unaryFactor_); @@ -164,7 +164,7 @@ namespace gtsam { namespace partition { NLG sepFactors; typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); - BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) + for(const sharedGenericUnaryFactor& unaryFactor_: unaryFactors) sepFactors.push_back(fg_[unaryFactor_->index]); return sepFactors; } diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index 9bdf40026..8c8f12d74 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -10,7 +10,6 @@ #include // for operator += #include // for operator += using namespace boost::assign; -#include #include #include @@ -89,10 +88,10 @@ TEST ( Partition, edgePartitionByMetis ) vector A_expected; A_expected += 0, 1; // frontal vector B_expected; B_expected += 2, 3; // frontal vector C_expected; // separator -// BOOST_FOREACH(const size_t a, actual->A) +// for(const size_t a: actual->A) // cout << a << " "; // cout << endl; -// BOOST_FOREACH(const size_t b, actual->B) +// for(const size_t b: actual->B) // cout << b << " "; // cout << endl; diff --git a/gtsam_unstable/partition/tests/testGenericGraph.cpp b/gtsam_unstable/partition/tests/testGenericGraph.cpp index c6e432902..8a32837f4 100644 --- a/gtsam_unstable/partition/tests/testGenericGraph.cpp +++ b/gtsam_unstable/partition/tests/testGenericGraph.cpp @@ -6,16 +6,17 @@ * Description: unit tests for generic graph */ -#include +#include + +#include + #include // for operator += #include // for operator += #include // for operator += using namespace boost::assign; -#include #include -#include -#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp index d7035c2d8..acaa5557e 100644 --- a/gtsam_unstable/partition/tests/testNestedDissection.cpp +++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp @@ -10,7 +10,6 @@ #include // for operator += #include // for operator += using namespace boost::assign; -#include #include #include diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 8519f6f8d..df6b1e50d 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -8,7 +8,6 @@ #include #include -#include using namespace boost::assign; @@ -29,7 +28,7 @@ DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t d /* ************************************************************************* */ void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s << " DummyFactor dim = " << rowDim_ << ", keys = { "; - BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } + for(Key key: this->keys()) { std::cout << keyFormatter(key) << " "; } std::cout << "}" << std::endl; } diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 7509fe3b2..0d10b0123 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -24,17 +24,17 @@ namespace gtsam { * Ternary factor representing a visual measurement that includes inverse depth */ template -class InvDepthFactor3: public gtsam::NoiseModelFactor3 { +class InvDepthFactor3: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O - gtsam::Point2 measured_; ///< 2D measurement - boost::shared_ptr K_; ///< shared pointer to calibration object + Point2 measured_; ///< 2D measurement + boost::shared_ptr K_; ///< shared pointer to calibration object public: /// shorthand for base class type - typedef gtsam::NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /// shorthand for this class typedef InvDepthFactor3 This; @@ -43,7 +43,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactor3() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -55,8 +57,8 @@ public: * @param invDepthKey is the index of inverse depth * @param K shared pointer to the constant calibration */ - InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model, - const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const Cal3_S2::shared_ptr& K) : + InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model, + const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) : Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {} /** Virtual destructor */ @@ -68,44 +70,43 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactor3", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } /// Evaluate error h(x)-z and optionally derivatives - gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const { + Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const { try { - InvDepthCamera3 camera(pose, K_); - gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_); - return reprojectionError.vector(); + InvDepthCamera3 camera(pose, K_); + return camera.project(point, invDepth, H1, H2, H3) - measured_; } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,5); if (H3) *H2 = Matrix::Zero(2,1); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0).finished(); + return (Vector(1) << 0.0).finished(); } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } /** return the calibration object */ - inline const gtsam::Cal3_S2::shared_ptr calibration() const { + inline const Cal3_S2::shared_ptr calibration() const { return K_; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index e9f894faf..ad66eee5b 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -41,7 +41,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactorVariant1() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactorVariant1() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -64,17 +66,17 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant1", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -86,22 +88,21 @@ public: Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); // Project landmark into Pose2 PinholeCamera camera(pose, *K_); - gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); - return reprojectionError.vector(); + return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0).finished(); + return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector6& landmark, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { if (H1) { (*H1) = numericalDerivative11( @@ -118,7 +119,7 @@ public: } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index ec2615ed6..c6b5d5af8 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -43,7 +43,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactorVariant2() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactorVariant2() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -67,17 +69,17 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant2", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol) && traits::Equals(this->referencePoint_, e->referencePoint_, tol); } @@ -89,22 +91,21 @@ public: Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); // Project landmark into Pose2 PinholeCamera camera(pose, *K_); - gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); - return reprojectionError.vector(); + return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0).finished(); + return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector3& landmark, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { if (H1) { (*H1) = numericalDerivative11( @@ -121,7 +122,7 @@ public: } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index cc5878d85..3041f5f23 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -41,7 +41,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactorVariant3a() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactorVariant3a() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -66,17 +68,17 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant3a", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -89,22 +91,21 @@ public: Point3 world_P_landmark = pose.transform_from(pose_P_landmark); // Project landmark into Pose2 PinholeCamera camera(pose, *K_); - gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); - return reprojectionError.vector(); + return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]" << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector3& landmark, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { if(H1) { (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); @@ -117,7 +118,7 @@ public: } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } @@ -160,7 +161,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactorVariant3b() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactorVariant3b() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -185,17 +188,17 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant3", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -208,23 +211,22 @@ public: Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark); // Project landmark into Pose2 PinholeCamera camera(pose2, *K_); - gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); - return reprojectionError.vector(); + return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]" << " moved behind camera " << DefaultKeyFormatter(this->key2()) << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const { + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const { if(H1) (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); @@ -239,7 +241,7 @@ public: } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 94126a68c..6112a398f 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -6,7 +6,6 @@ */ #include "Mechanization_bRn2.h" -#include namespace gtsam { diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index dc250fd9d..3e1263bb9 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -101,9 +101,9 @@ namespace gtsam { virtual ~MultiProjectionFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } /** * print @@ -143,20 +143,20 @@ namespace gtsam { // // if(body_P_sensor_) { // if(H1) { -// gtsam::Matrix H0; +// Matrix H0; // PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); // Point2 reprojectionError(camera.project(point, H1, H2) - measured_); // *H1 = *H1 * H0; -// return reprojectionError.vector(); +// return reprojectionError; // } else { // PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); // Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// return reprojectionError.vector(); +// return reprojectionError; // } // } else { // PinholeCamera camera(pose, *K_); // Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// return reprojectionError.vector(); +// return reprojectionError; // } // } @@ -168,20 +168,20 @@ namespace gtsam { try { if(body_P_sensor_) { if(H1) { - gtsam::Matrix H0; + Matrix H0; PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); Point2 reprojectionError(camera.project(point, H1, H2) - measured_); *H1 = *H1 * H0; - return reprojectionError.vector(); + return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); Point2 reprojectionError(camera.project(point, H1, H2) - measured_); - return reprojectionError.vector(); + return reprojectionError; } } else { PinholeCamera camera(pose, *K_); Point2 reprojectionError(camera.project(point, H1, H2) - measured_); - return reprojectionError.vector(); + return reprojectionError; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index adfc1d108..19b8d56e6 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -54,7 +54,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Default constructor - ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {} + ProjectionFactorPPP() : + measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) { + } /** * Constructor @@ -94,9 +96,9 @@ namespace gtsam { virtual ~ProjectionFactorPPP() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } /** * print @@ -105,7 +107,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "ProjectionFactorPPP, z = "; - measured_.print(); + traits::Print(measured_); Base::print("", keyFormatter); } @@ -114,7 +116,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -125,16 +127,15 @@ namespace gtsam { boost::optional H3 = boost::none) const { try { if(H1 || H2 || H3) { - gtsam::Matrix H0, H02; + Matrix H0, H02; PinholeCamera camera(pose.compose(transform, H0, H02), *K_); Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); *H2 = *H1 * H02; *H1 = *H1 * H0; - return reprojectionError.vector(); + return reprojectionError; } else { PinholeCamera camera(pose.compose(transform), *K_); - Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); - return reprojectionError.vector(); + return camera.project(point, H1, H3, boost::none) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 2fd622ea1..369075abf 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -52,7 +52,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Default constructor - ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {} + ProjectionFactorPPPC() : + measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) { + } /** * Constructor @@ -89,9 +91,9 @@ namespace gtsam { virtual ~ProjectionFactorPPPC() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } /** * print @@ -100,7 +102,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "ProjectionFactorPPPC, z = "; - measured_.print(); + traits::Print(measured_); Base::print("", keyFormatter); } @@ -109,7 +111,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol); + && traits::Equals(this->measured_, e->measured_, tol); } /// Evaluate error h(x)-z and optionally derivatives @@ -120,16 +122,15 @@ namespace gtsam { boost::optional H4 = boost::none) const { try { if(H1 || H2 || H3 || H4) { - gtsam::Matrix H0, H02; + Matrix H0, H02; PinholeCamera camera(pose.compose(transform, H0, H02), K); Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_); *H2 = *H1 * H02; *H1 = *H1 * H0; - return reprojectionError.vector(); + return reprojectionError; } else { PinholeCamera camera(pose.compose(transform), K); - Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_); - return reprojectionError.vector(); + return camera.project(point, H1, H3, H4) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 3db302d0b..5e107ea58 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -14,7 +14,6 @@ #include #include #include -#include #include namespace gtsam { @@ -100,13 +99,13 @@ public: boost::optional best_circle; // loop over all circles - BOOST_FOREACH(const Circle2& it, circles) { + for(const Circle2& it: circles) { // distance between circle centers. - double d = circle1.center.dist(it.center); + double d = distance2(circle1.center, it.center); if (d < 1e-9) continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles - boost::optional fh = Point2::CircleCircleIntersection( + boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); // Check if this pair is better by checking h = fh->y() // if h is large, the intersections are well defined. @@ -117,15 +116,15 @@ public: } // use best fh to find actual intersection points - std::list intersections = Point2::CircleCircleIntersection( + std::list intersections = circleCircleIntersection( circle1.center, best_circle->center, best_fh); // pick winner based on other measurements double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); - BOOST_FOREACH(const Circle2& it, circles) { - error1 += it.center.dist(p1); - error2 += it.center.dist(p2); + for(const Circle2& it: circles) { + error1 += distance2(it.center, p1); + error2 += distance2(it.center, p2); } return (error1 < error2) ? p1 : p2; //gttoc_(triangulate); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 56e22aae2..29ae6233b 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -21,6 +21,7 @@ #pragma once #include +#include #include #include @@ -35,91 +36,10 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to - enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD - }; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY - }; - - /* - * Parameters for the smart stereo projection factors - */ - struct GTSAM_EXPORT SmartStereoProjectionParams { - - LinearizationMode linearizationMode; ///< How to linearize the factor - DegeneracyMode degeneracyMode; ///< How to linearize the factor - - /// @name Parameters governing the triangulation - /// @{ - TriangulationParameters triangulation; - double retriangulationThreshold; ///< threshold to decide whether to re-triangulate - /// @} - - /// @name Parameters governing how triangulation result is treated - /// @{ - bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) - bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - - - /// Constructor - SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false) : - linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - 1e-5), throwCheirality(throwCheirality), verboseCheirality( - verboseCheirality) { - } - - virtual ~SmartStereoProjectionParams() { - } - - void print(const std::string& str) const { - std::cout << "linearizationMode: " << linearizationMode << "\n"; - std::cout << " degeneracyMode: " << degeneracyMode << "\n"; - std::cout << triangulation << std::endl; - } - - LinearizationMode getLinearizationMode() const { - return linearizationMode; - } - DegeneracyMode getDegeneracyMode() const { - return degeneracyMode; - } - TriangulationParameters getTriangulationParameters() const { - return triangulation; - } - bool getVerboseCheirality() const { - return verboseCheirality; - } - bool getThrowCheirality() const { - return throwCheirality; - } - void setLinearizationMode(LinearizationMode linMode) { - linearizationMode = linMode; - } - void setDegeneracyMode(DegeneracyMode degMode) { - degeneracyMode = degMode; - } - void setRankTolerance(double rankTol) { - triangulation.rankTolerance = rankTol; - } - void setEnableEPI(bool enableEPI) { - triangulation.enableEPI = enableEPI; - } - void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { - triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; - } - void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { - triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; - } - }; - - +/* + * Parameters for the smart stereo projection factors (identical to the SmartProjectionParams) + */ +typedef SmartProjectionParams SmartStereoProjectionParams; /** * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. @@ -155,14 +75,19 @@ public: /// Vector of cameras typedef CameraSet Cameras; + /// Vector of monocular cameras (stereo treated as 2 monocular) + typedef PinholeCamera MonoCamera; + typedef CameraSet MonoCameras; + typedef std::vector MonoMeasurements; + /** * Constructor * @param params internal parameters of the smart factors */ SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()) : - Base(sharedNoiseModel), // + const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), + const boost::optional body_P_sensor = boost::none) : + Base(sharedNoiseModel, body_P_sensor), // params_(params), // result_(TriangulationResult::Degenerate()) { } @@ -240,75 +165,28 @@ public: size_t m = cameras.size(); bool retriangulate = decideIfTriangulate(cameras); -// if(!retriangulate) -// std::cout << "retriangulate = false" << std::endl; -// -// bool retriangulate = true; - - if (retriangulate) { -// std::cout << "Retriangulate " << std::endl; - std::vector reprojections; - reprojections.reserve(m); - for(size_t i = 0; i < m; i++) { - reprojections.push_back(cameras[i].backproject(measured_[i])); + // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras + MonoCameras monoCameras; + MonoMeasurements monoMeasured; + for(size_t i = 0; i < m; i++) { + const Pose3 leftPose = cameras[i].pose(); + const Cal3_S2 monoCal = cameras[i].calibration().calibration(); + const MonoCamera leftCamera_i(leftPose,monoCal); + const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0)); + const Pose3 rightPose = leftPose.compose( left_Pose_right ); + const MonoCamera rightCamera_i(rightPose,monoCal); + const StereoPoint2 zi = measured_[i]; + monoCameras.push_back(leftCamera_i); + monoMeasured.push_back(Point2(zi.uL(),zi.v())); + if(!std::isnan(zi.uR())){ // if right point is valid + monoCameras.push_back(rightCamera_i); + monoMeasured.push_back(Point2(zi.uR(),zi.v())); } - - Point3 pw_sum(0,0,0); - BOOST_FOREACH(const Point3& pw, reprojections) { - pw_sum = pw_sum + pw; - } - // average reprojected landmark - Point3 pw_avg = pw_sum / double(m); - - double totalReprojError = 0; - - // check if it lies in front of all cameras - for(size_t i = 0; i < m; i++) { - const Pose3& pose = cameras[i].pose(); - const Point3& pl = pose.transform_to(pw_avg); - if (pl.z() <= 0) { - result_ = TriangulationResult::BehindCamera(); - return result_; - } - - // check landmark distance - if (params_.triangulation.landmarkDistanceThreshold > 0 && - pl.norm() > params_.triangulation.landmarkDistanceThreshold) { - result_ = TriangulationResult::Degenerate(); - return result_; - } - - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) { - const StereoPoint2& zi = measured_[i]; - StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi); - totalReprojError += reprojectionError.vector().norm(); - } - } // for - - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) { - result_ = TriangulationResult::Degenerate(); - return result_; - } - - if(params_.triangulation.enableEPI) { - try { - pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); - } catch(StereoCheiralityException& e) { - if(params_.verboseCheirality) - std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl; - if(params_.throwCheirality) - throw; - result_ = TriangulationResult::BehindCamera(); - return TriangulationResult::BehindCamera(); - } - } - - result_ = TriangulationResult(pw_avg); - - } // if retriangulate + } + if (retriangulate) + result_ = gtsam::triangulateSafe(monoCameras, monoMeasured, + params_.triangulation); return result_; - } /// triangulate @@ -339,9 +217,9 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian - BOOST_FOREACH(Matrix& m, Gs) + for(Matrix& m: Gs) m = Matrix::Zero(Base::Dim, Base::Dim); - BOOST_FOREACH(Vector& v, gs) + for(Vector& v: gs) v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, Gs, gs, 0.0); @@ -570,6 +448,32 @@ public: } } + /** + * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) + */ + virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const + { + // when using stereo cameras, some of the measurements might be missing: + for(size_t i=0; i < cameras.size(); i++){ + const StereoPoint2& z = measured_.at(i); + if(std::isnan(z.uR())) // if the right pixel is invalid + { + if(Fs){ // delete influence of right point on jacobian Fs + MatrixZD& Fi = Fs->at(i); + for(size_t ii=0; iirow(ZDim * i + 1) = Matrix::Zero(1, E->cols()); + + // set the corresponding entry of vector ue to zero + ue(ZDim * i + 1) = 0.0; + } + } + } + /** return the landmark */ TriangulationResult point() const { return result_; @@ -582,19 +486,19 @@ public: } /// Is result valid? - bool isValid() const { - return bool(result_); - } + bool isValid() const { return result_.valid(); } /** return the degenerate state */ - bool isDegenerate() const { - return result_.degenerate(); - } + bool isDegenerate() const { return result_.degenerate(); } /** return the cheirality status flag */ - bool isPointBehindCamera() const { - return result_.behindCamera(); - } + bool isPointBehindCamera() const { return result_.behindCamera(); } + + /** return the outlier state */ + bool isOutlier() const { return result_.outlier(); } + + /** return the farPoint state */ + bool isFarPoint() const { return result_.farPoint(); } private: diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index c2526fd74..3b4c5e0db 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -66,9 +66,9 @@ public: * @param params internal parameters of the smart factors */ SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()) : - Base(sharedNoiseModel, params) { + const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), + const boost::optional body_P_sensor = boost::none) : + Base(sharedNoiseModel, params, body_P_sensor) { } /** Virtual destructor */ @@ -102,7 +102,7 @@ public: /** * Variant of the previous one in which we include a set of measurements with the same noise and calibration - * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKeys vector of keys corresponding to the camera observing the same landmark * @param K the (known) camera calibration (same for all measurements) */ @@ -122,7 +122,7 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartStereoProjectionPoseFactor, z = \n "; - BOOST_FOREACH(const boost::shared_ptr& K, K_all_) + for(const boost::shared_ptr& K: K_all_) K->print("calibration = "); Base::print("", keyFormatter); } @@ -160,8 +160,12 @@ public: Base::Cameras cameras(const Values& values) const { Base::Cameras cameras; size_t i=0; - BOOST_FOREACH(const Key& k, this->keys_) { - const Pose3& pose = values.at(k); + for(const Key& k: this->keys_) { + Pose3 pose = values.at(k); + + if (Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); + StereoCamera camera(pose, K_all_[i++]); cameras.push_back(camera); } diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index aae4e413d..6c2f55195 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -48,9 +48,7 @@ public: Vector evaluateError(const Pose2& pose, const Point2& point, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - Point2 d = pose.transform_to(point, H1, H2); - Point2 e = d - measured_; - return e.vector(); + return pose.transform_to(point, H1, H2) - measured_; } }; @@ -99,12 +97,12 @@ public: *H3 = D_e_point_g * D_point_g_base2; if (H4) *H4 = D_e_point_g * D_point_g_point; - return (d - measured_).vector(); + return d - measured_; } else { Pose2 pose_g = base1.compose(pose); Point2 point_g = base2.transform_from(point); Point2 d = pose_g.transform_to(point_g); - return (d - measured_).vector(); + return d - measured_; } } }; diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 9ae4aa928..e18505af6 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -116,8 +116,8 @@ TEST( SmartRangeFactor, optimization ) { graph.push_back(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); - graph.push_back(PriorFactor(1, pose1, priorNoise)); - graph.push_back(PriorFactor(2, pose2, priorNoise)); + graph.emplace_shared >(1, pose1, priorNoise); + graph.emplace_shared >(2, pose2, priorNoise); // Try optimizing LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 5bad0e171..a018ec7ee 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,7 +18,7 @@ * @date Sept 2013 */ -// TODO #include +#include #include #include #include @@ -33,8 +33,6 @@ using namespace boost::assign; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w = 640, h = 480; static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); @@ -62,6 +60,8 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), 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) { @@ -79,6 +79,35 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, LevenbergMarquardtParams lm_params; +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, params) { + SmartStereoProjectionParams p; + + // check default values and "get" + EXPECT(p.getLinearizationMode() == HESSIAN); + EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY); + EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9); + EXPECT(p.getVerboseCheirality() == false); + EXPECT(p.getThrowCheirality() == false); + + // check "set" + p.setLinearizationMode(JACOBIAN_SVD); + p.setDegeneracyMode(ZERO_ON_DEGENERACY); + p.setRankTolerance(100); + p.setEnableEPI(true); + p.setLandmarkDistanceThreshold(200); + p.setDynamicOutlierRejectionThreshold(3); + p.setRetriangulationThreshold(1e-2); + + EXPECT(p.getLinearizationMode() == JACOBIAN_SVD); + EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5); + EXPECT(p.getTriangulationParameters().enableEPI == true); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5); + EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5); +} + /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); @@ -151,6 +180,60 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { //EXPECT(assert_equal(zero(4),actual,1e-8)); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) { + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); + StereoCamera level_camera(level_pose, K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera level_camera_right(level_pose_right, K2); + + // landmark ~5 meters in front of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + StereoPoint2 level_uv = level_camera.project(landmark); + StereoPoint2 level_uv_right = level_camera_right.project(landmark); + StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v()); + + Values values; + values.insert(x1, level_pose); + values.insert(x2, level_pose_right); + + SmartStereoProjectionPoseFactor factor1(model); + factor1.add(level_uv, x1, K2); + factor1.add(level_uv_right_missing, x2, K2); + + double actualError = factor1.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing: + SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values); + double actualError2 = factor1.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + CameraSet cams; + cams += level_camera; + cams += level_camera_right; + TriangulationResult result = factor1.triangulateSafe(cams); + CHECK(result); + EXPECT(assert_equal(landmark, *result, 1e-7)); + + // TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing: + SmartStereoProjectionPoseFactor factor2(model); + StereoPoint2 level_uv_missing(level_uv.uL(),missing_uR,level_uv.v()); + factor2.add(level_uv_missing, x1, K2); + factor2.add(level_uv_right_missing, x2, K2); + result = factor2.triangulateSafe(cams); + CHECK(result); + EXPECT(assert_equal(landmark, *result, 1e-7)); +} + /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) @@ -248,14 +331,12 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -273,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { Point3(0.1, -0.1, 1.9)), values.at(x3))); // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7); + EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error // get triangulated landmarks from smart factors Point3 landmark1_smart = *smartFactor1->point(); @@ -335,7 +416,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality)); // cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7); + EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values), 1e-7); LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params); Values result2 = optimizer2.optimize(); @@ -344,7 +425,192 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl; } +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) { + // camera has some displacement + Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.01, 0., -0.05), Point3(0.1, 0, 0.1)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1.compose(body_P_sensor), K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2.compose(body_P_sensor), K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3.compose(body_P_sensor), K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_l1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_l2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_l3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartStereoProjectionParams smart_params; + smart_params.triangulation.enableEPI = true; + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor1->add(measurements_l1, views, K2); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor2->add(measurements_l2, views, K2); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor3->add(measurements_l3, views, K2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; + EXPECT_DOUBLES_EQUAL(953392.32838422502, graph.error(values), 1e-7); // initial error + + Values result; + gttic_(SmartStereoProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + gttoc_(SmartStereoProjectionPoseFactor); + tictoc_finishedIteration_(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + EXPECT(assert_equal(pose3, result.at(x3))); +} +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){ + // make a realistic calibration matrix + double fov = 60; // degrees + size_t w=640,h=480; + + Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); + + SimpleCamera cam1(cameraPose1, *K); // with camera poses + SimpleCamera cam2(cameraPose2, *K); + SimpleCamera cam3(cameraPose3, *K); + + // create arbitrary body_Pose_sensor (transforms from sensor to body) + Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + + // These are the poses we want to estimate, from camera measurements + Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); + Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse()); + Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse()); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(5, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN) + vector measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo; + for(size_t k=0; k(x1, bodyPose1, noisePrior)); + graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, bodyPose1); + gtValues.insert(x2, bodyPose2); + gtValues.insert(x3, bodyPose3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Values values; + values.insert(x1, bodyPose1); + values.insert(x2, bodyPose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, bodyPose3*noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(bodyPose3,result.at(x3))); +} /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { @@ -390,6 +656,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(x2, pose2, noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3 * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + // DELETE SOME MEASUREMENTS + StereoPoint2 sp = measurements_cam1[1]; + measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + sp = measurements_cam2[2]; + measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); @@ -408,7 +746,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3),1e-7)); } /* *************************************************************************/ @@ -463,8 +801,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -545,8 +883,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(x2, pose2, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -562,13 +900,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); // dynamic outlier rejection is off - EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9); // Factors 1-3 should have valid point, factor 4 should not EXPECT(smartFactor1->point()); EXPECT(smartFactor2->point()); EXPECT(smartFactor3->point()); - EXPECT(smartFactor4->point().degenerate()); + EXPECT(smartFactor4->point().outlier()); EXPECT(smartFactor4b->point()); // Factor 4 is disabled, pose 3 stays put @@ -1039,7 +1377,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { vector views; views.push_back(x1); @@ -1072,6 +1410,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); + // check that it is non degenerate + EXPECT(smartFactor->isValid()); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; @@ -1082,6 +1423,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); + // check that it is non degenerate + EXPECT(smartFactor->isValid()); + // Hessian is invariant to rotations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), @@ -1098,10 +1442,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); - // Hessian is invariant to rotations and translations in the nondegenerate case + // Hessian is invariant to rotations and translations in the degenerate case EXPECT( assert_equal(hessianFactor->information(), +#ifdef GTSAM_USE_EIGEN_MKL + hessianFactorRotTran->information(), 1e-5)); +#else hessianFactorRotTran->information(), 1e-6)); +#endif } /* ************************************************************************* */ diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index f0bbb9072..f39bdda59 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -46,7 +45,7 @@ int main(int argc, char* argv[]) { // loop over number of images vector ms; ms += 10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000; - BOOST_FOREACH(size_t m,ms) { + for(size_t m: ms) { // We use volatile here to make these appear to the optimizing compiler as // if their values are only known at run-time. volatile size_t n = 500; // number of points per image @@ -74,7 +73,7 @@ int main(int argc, char* argv[]) { // DSFBase version timer tim; DSFBase dsf(N); // Allow for N keys - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.merge(m.first, m.second); os << tim.elapsed() << ","; cout << format("DSFBase: %1% s") % tim.elapsed() << endl; @@ -84,7 +83,7 @@ int main(int argc, char* argv[]) { // DSFMap version timer tim; DSFMap dsf; - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.merge(m.first, m.second); os << tim.elapsed() << endl; cout << format("DSFMap: %1% s") % tim.elapsed() << endl; @@ -96,7 +95,7 @@ int main(int argc, char* argv[]) { DSF dsf; for (size_t j = 0; j < N; j++) dsf = dsf.makeSet(j); - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf = dsf.makeUnion(m.first, m.second); os << tim.elapsed() << endl; cout << format("DSF functional: %1% s") % tim.elapsed() << endl; @@ -108,7 +107,7 @@ int main(int argc, char* argv[]) { DSF dsf; for (size_t j = 0; j < N; j++) dsf.makeSetInPlace(j); - BOOST_FOREACH(const Match& m, matches) + for(const Match& m: matches) dsf.makeUnionInPlace(m.first, m.second); os << tim.elapsed() << ","; cout << format("DSF in-place: %1% s") % tim.elapsed() << endl; diff --git a/matlab.h b/matlab.h index 72889dc4b..5e144730d 100644 --- a/matlab.h +++ b/matlab.h @@ -91,7 +91,7 @@ Matrix extractPoint2(const Values& values) { Values::ConstFiltered points = values.filter(); Matrix result(points.size(), 2); for(const auto& key_value: points) - result.row(j++) = key_value.value.vector(); + result.row(j++) = key_value.value; return result; } diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt index 6f74f9806..a1691be32 100644 --- a/matlab/README-gtsam-toolbox.txt +++ b/matlab/README-gtsam-toolbox.txt @@ -7,8 +7,14 @@ http://borg.cc.gatech.edu/projects/gtsam ================================================================================ This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ -library. +library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake. +The interface to the toolbox is generated automatically by the wrap +tool which directly parses C++ header files. The tool generates matlab +proxy objects together with all the native functions for wrapping and +unwrapping scalar and non scalar types and objects. The interface +generated by the wrap tool also redirects the standard output stream +(cout) to matlab's console. ---------------------------------------- Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04) diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 2e95ea33a..2d7ac72f7 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1 +1 @@ -from gtsampy import * +from _gtsampy import * diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py index 0c80c2fb5..781dae118 100644 --- a/python/gtsam_examples/ImuFactorExample.py +++ b/python/gtsam_examples/ImuFactorExample.py @@ -69,7 +69,7 @@ class ImuFactorExample(PreintegrationExample): # get measurements and add them to PIM measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) - pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt, H9, H9) + pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) # Plot IMU many times if k % 10 == 0: diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py index 6b0d83b10..b441ffecb 100644 --- a/python/gtsam_examples/PreintegrationExample.py +++ b/python/gtsam_examples/PreintegrationExample.py @@ -101,7 +101,7 @@ class PreintegrationExample(object): actualPose = self.scenario.pose(t) plotPose3(POSES_FIG, actualPose, 0.3) t = actualPose.translation() - self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) + self.maxDim = max([abs(t[0]), abs(t[1]), abs(t[2]), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) diff --git a/python/gtsam_utils/plot.py b/python/gtsam_utils/plot.py index 01ec85009..8863f427e 100644 --- a/python/gtsam_utils/plot.py +++ b/python/gtsam_utils/plot.py @@ -33,7 +33,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None): def plotPose3OnAxes(ax, pose, axisLength=0.1): # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global - C = pose.translation().vector() + C = pose.translation() # draw the camera axes xAxis = C + gRp[:, 0] * axisLength diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt index 00900206c..689354b4e 100644 --- a/python/handwritten/CMakeLists.txt +++ b/python/handwritten/CMakeLists.txt @@ -12,23 +12,30 @@ endforeach() add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper) set_target_properties(gtsam_python PROPERTIES - OUTPUT_NAME gtsampy + OUTPUT_NAME _gtsampy PREFIX "" ${build_type_toupper}_POSTFIX "" SKIP_BUILD_RPATH TRUE CLEAN_DIRECT_OUTPUT 1 ) + +target_include_directories(gtsam_python SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR}) +target_include_directories(gtsam_python SYSTEM PUBLIC ${NUMPY_INCLUDE_DIRS}) +target_include_directories(gtsam_python SYSTEM PUBLIC ${PYTHON_INCLUDE_DIRS}) +target_include_directories(gtsam_python SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) +target_include_directories(gtsam_python SYSTEM PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/) + target_link_libraries(gtsam_python ${Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_LIBRARY} ${PYTHON_LIBRARY} gtsam) # Cause the library to be output in the correct directory. # TODO: Change below to work on different systems (currently works only with Linux) -set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_libgtsam_python.so) +set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_gtsampy.so) add_custom_command( OUTPUT ${output_path} DEPENDS gtsam_python COMMAND ${CMAKE_COMMAND} -E copy $ ${output_path} - COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" + COMMENT "Copying extension module to python/gtsam/_gtsampy.so" ) add_custom_target(copy_gtsam_python_module ALL DEPENDS ${output_path}) \ No newline at end of file diff --git a/python/handwritten/common.h b/python/handwritten/common.h new file mode 100644 index 000000000..72d2ae846 --- /dev/null +++ b/python/handwritten/common.h @@ -0,0 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief common macros used by handwritten exports of the python module + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#pragma once + + /* Fix to avoid registration warnings */ +// Solution taken from https://github.com/BVLC/caffe/pull/4069/commits/673e8cfc0b8f05f9fa3ebbad7cc6202822e5d9c5 +#define REGISTER_SHARED_PTR_TO_PYTHON(PTR) do { \ + const boost::python::type_info info = \ + boost::python::type_id >(); \ + const boost::python::converter::registration* reg = \ + boost::python::converter::registry::query(info); \ + if (reg == NULL) { \ + boost::python::register_ptr_to_python >(); \ + } else if ((*reg).m_to_python == NULL) { \ + boost::python::register_ptr_to_python >(); \ + } \ +} while (0) diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp index 94dc10e56..8fa7e0fdd 100644 --- a/python/handwritten/exportgtsam.cpp +++ b/python/handwritten/exportgtsam.cpp @@ -62,7 +62,7 @@ void registerNumpyEigenConversions(); //-----------------------------------// -BOOST_PYTHON_MODULE(gtsampy){ +BOOST_PYTHON_MODULE(_gtsampy){ // NOTE: We need to call import_array1() instead of import_array() to support both python 2 // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function diff --git a/python/handwritten/geometry/Cal3_S2.cpp b/python/handwritten/geometry/Cal3_S2.cpp index 339cd6a3c..440b21742 100644 --- a/python/handwritten/geometry/Cal3_S2.cpp +++ b/python/handwritten/geometry/Cal3_S2.cpp @@ -19,7 +19,7 @@ #define NO_IMPORT_ARRAY #include -#include "gtsam/geometry/Cal3_S2.h" +#include using namespace boost::python; using namespace gtsam; @@ -36,11 +36,12 @@ Vector3 (Cal3_S2::*calibrate2)(const Vector3 &) const = &Cal3_S2::calibrate; void exportCal3_S2(){ -class_("Cal3_S2", init<>()) +class_ >("Cal3_S2", init<>()) .def(init()) .def(init()) - .def(init()) + .def(init(args("fov","w","h"))) .def(init()) + .def(repr(self)) .def("print", &Cal3_S2::print, print_overloads(args("s"))) .def("equals", &Cal3_S2::equals, equals_overloads(args("q","tol"))) .def("fx",&Cal3_S2::fx) @@ -58,5 +59,6 @@ class_("Cal3_S2", init<>()) .def("calibrate",calibrate2) .def("between",&Cal3_S2::between, between_overloads()) ; +register_ptr_to_python< boost::shared_ptr >(); -} \ No newline at end of file +} diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp index 7af3f8cf6..99a97adc9 100644 --- a/python/handwritten/geometry/Point2.cpp +++ b/python/handwritten/geometry/Point2.cpp @@ -25,23 +25,26 @@ using namespace boost::python; using namespace gtsam; +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(distance_overloads, Point2::distance, 1, 3) +#endif void exportPoint2(){ +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS class_("Point2", init<>()) .def(init()) .def(init()) .def("identity", &Point2::identity) - .def("dist", &Point2::dist) - .def("distance", &Point2::distance) + .def("distance", &Point2::distance, distance_overloads(args("q","H1","H2"))) .def("equals", &Point2::equals, equals_overloads(args("q","tol"))) .def("norm", &Point2::norm) .def("print", &Point2::print, print_overloads(args("s"))) .def("unit", &Point2::unit) - .def("vector", &Point2::vector) + .def("vector", &Point2::vector, return_value_policy()) .def("x", &Point2::x) .def("y", &Point2::y) .def(self * other()) // __mult__ @@ -54,5 +57,5 @@ void exportPoint2(){ .def(repr(self)) .def(self == self) ; - -} \ No newline at end of file +#endif +} diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index de5c8e556..7935d6b37 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -25,31 +25,32 @@ using namespace boost::python; using namespace gtsam; +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(norm_overloads, Point3::norm, 0, 1) +#endif void exportPoint3(){ +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS class_("Point3") .def(init<>()) .def(init()) .def(init()) - .def("identity", &Point3::identity) - .staticmethod("identity") - .def("cross", &Point3::cross) - .def("distance", &Point3::distance) - .def("dot", &Point3::dot) - .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) - .def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>"))) - .def("normalized", &Point3::normalized) - .def("print", &Point3::print, print_overloads(args("s"))) -#ifndef GTSAM_USE_VECTOR3_POINTS .def("vector", &Point3::vector, return_value_policy()) .def("x", &Point3::x) .def("y", &Point3::y) .def("z", &Point3::z) -#endif + .def("print", &Point3::print, print_overloads(args("s"))) + .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) + .def("distance", &Point3::distance) + .def("cross", &Point3::cross) + .def("dot", &Point3::dot) + .def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>"))) + .def("normalized", &Point3::normalized) + .def("identity", &Point3::identity) + .staticmethod("identity") .def(self * other()) .def(other() * self) .def(self + self) @@ -58,7 +59,10 @@ class_("Point3") .def(self / other()) .def(self_ns::str(self)) .def(repr(self)) - .def(self == self) -; + .def(self == self); +#endif +class_("Point3Pair", init()) + .def_readwrite("first", &Point3Pair::first) + .def_readwrite("second", &Point3Pair::second); } diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp index f778ea4e0..551f2f60c 100644 --- a/python/handwritten/geometry/Pose3.cpp +++ b/python/handwritten/geometry/Pose3.cpp @@ -53,6 +53,9 @@ void exportPose3(){ // function pointers to desambiguate range() calls double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const = &Pose3::range; double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; + // function pointers to desambiguate bearing() calls + Unit3 (Pose3::*bearing1)(const Point3 &, OptionalJacobian<2,6>, OptionalJacobian<2,3>) const = &Pose3::bearing; + Unit3 (Pose3::*bearing2)(const Pose3 &, OptionalJacobian<2,6>, OptionalJacobian<2,6>) const = &Pose3::bearing; class_("Pose3") .def(init<>()) @@ -65,7 +68,6 @@ void exportPose3(){ .def("equals", &Pose3::equals, equals_overloads(args("pose", "tol"))) .def("identity", &Pose3::identity) .staticmethod("identity") - .def("bearing", &Pose3::bearing) .def("matrix", &Pose3::matrix) .def("transform_from", &Pose3::transform_from, transform_from_overloads(args("point", "H1", "H2"))) @@ -88,5 +90,6 @@ void exportPose3(){ .def("between", between2, between_overloads()) .def("range", range1, range_overloads()) .def("range", range2, range_overloads()) - .def("bearing", &Pose3::bearing, bearing_overloads()); + .def("bearing", bearing1, bearing_overloads()) + .def("bearing", bearing2, bearing_overloads()); } diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index b840718a3..440559e3e 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -53,6 +53,8 @@ void exportRot3(){ class_("Rot3") .def(init()) .def(init()) + .def(init()) + .def(init()) .def(init()) .def(init()) .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") @@ -81,6 +83,8 @@ void exportRot3(){ .def("RzRyRx", RzRyRx_0, (arg("x"),arg("y"),arg("z")), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) .def("RzRyRx", RzRyRx_1, arg("xyz"), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) .staticmethod("RzRyRx") + .def("Ypr", &Rot3::Ypr) + .staticmethod("Ypr") .def("identity", &Rot3::identity) .staticmethod("identity") .def("AdjointMap", &Rot3::AdjointMap) diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp index 3612f7e14..e918e509d 100644 --- a/python/handwritten/linear/NoiseModel.cpp +++ b/python/handwritten/linear/NoiseModel.cpp @@ -28,6 +28,8 @@ #include "gtsam/linear/NoiseModel.h" +#include "python/handwritten/common.h" + using namespace boost::python; using namespace gtsam; using namespace gtsam::noiseModel; @@ -100,6 +102,7 @@ void exportNoiseModels(){ class_("Base") .def("print", pure_virtual(&Base::print)) ; + register_ptr_to_python< boost::shared_ptr >(); // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining) class_, bases >("Gaussian", no_init) @@ -110,7 +113,7 @@ void exportNoiseModels(){ .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads()) .staticmethod("Covariance") ; - register_ptr_to_python< boost::shared_ptr >(); + REGISTER_SHARED_PTR_TO_PYTHON(Gaussian); class_, bases >("Diagonal", no_init) .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) @@ -120,7 +123,7 @@ void exportNoiseModels(){ .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads()) .staticmethod("Precisions") ; - register_ptr_to_python< boost::shared_ptr >(); + REGISTER_SHARED_PTR_TO_PYTHON(Diagonal); class_, bases >("Isotropic", no_init) .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) @@ -130,12 +133,12 @@ void exportNoiseModels(){ .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads()) .staticmethod("Precision") ; - register_ptr_to_python< boost::shared_ptr >(); + REGISTER_SHARED_PTR_TO_PYTHON(Isotropic); class_, bases >("Unit", no_init) .def("Create",&Unit::Create) .staticmethod("Create") ; - register_ptr_to_python< boost::shared_ptr >(); + REGISTER_SHARED_PTR_TO_PYTHON(Unit); } diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index 2d7e36f47..0cf3062b5 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -22,6 +22,8 @@ #include "gtsam/navigation/ImuFactor.h" #include "gtsam/navigation/GPSFactor.h" +#include "python/handwritten/common.h" + using namespace boost::python; using namespace gtsam; @@ -78,17 +80,21 @@ void exportImuFactor() { .staticmethod("MakeSharedU"); // NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html - register_ptr_to_python< boost::shared_ptr >(); + REGISTER_SHARED_PTR_TO_PYTHON(PreintegrationParams); - class_( - "PreintegrationBase", + class_( +#ifdef GTSAM_TANGENT_PREINTEGRATION + "TangentPreintegration", +#else + "ManifoldPreintegration", +#endif init&, const imuBias::ConstantBias&>()) - .def("predict", &PreintegrationBase::predict, predict_overloads()) - .def("computeError", &PreintegrationBase::computeError) - .def("resetIntegration", &PreintegrationBase::resetIntegration) - .def("deltaTij", &PreintegrationBase::deltaTij); + .def("predict", &PreintegrationType::predict, predict_overloads()) + .def("computeError", &PreintegrationType::computeError) + .def("resetIntegration", &PreintegrationType::resetIntegration) + .def("deltaTij", &PreintegrationType::deltaTij); - class_>( + class_>( "PreintegratedImuMeasurements", init&, const imuBias::ConstantBias&>()) .def(repr(self)) @@ -101,21 +107,21 @@ void exportImuFactor() { .def("error", &ImuFactor::error) .def(init()) .def(repr(self)); - register_ptr_to_python>(); + REGISTER_SHARED_PTR_TO_PYTHON(ImuFactor); class_, boost::shared_ptr>("ImuFactor2") .def("error", &ImuFactor2::error) .def(init()) .def(repr(self)); - register_ptr_to_python>(); + REGISTER_SHARED_PTR_TO_PYTHON(ImuFactor2); class_, boost::shared_ptr>("GPSFactor") .def("error", &GPSFactor::error) .def(init()); - register_ptr_to_python>(); + REGISTER_SHARED_PTR_TO_PYTHON(GPSFactor); class_, boost::shared_ptr>("GPSFactor2") .def("error", &GPSFactor2::error) .def(init()); - register_ptr_to_python>(); + REGISTER_SHARED_PTR_TO_PYTHON(GPSFactor2); } diff --git a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp index f1c6a0b73..58e1d116a 100644 --- a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -15,7 +15,7 @@ void exportLevenbergMarquardtOptimizer() { .def_readwrite("maxIterations", &GaussNewtonParams::maxIterations) .def_readwrite("relativeErrorTol", &GaussNewtonParams::relativeErrorTol); - class_( + class_( "GaussNewtonOptimizer", init()) .def("optimize", &GaussNewtonOptimizer::optimize, @@ -31,7 +31,7 @@ void exportLevenbergMarquardtOptimizer() { .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM); - class_( + class_( "LevenbergMarquardtOptimizer", init()) .def("optimize", &LevenbergMarquardtOptimizer::optimize, diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp index 9a130d8e9..8ccc123fc 100644 --- a/python/handwritten/nonlinear/NonlinearFactor.cpp +++ b/python/handwritten/nonlinear/NonlinearFactor.cpp @@ -28,7 +28,7 @@ using namespace gtsam; // All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the // overloading through inheritance in Python. // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions -struct NonlinearFactorCallback : NonlinearFactor, wrapper +struct NonlinearFactorWrap : NonlinearFactor, wrapper { double error (const Values & values) const { return this->get_override("error")(values); @@ -41,9 +41,35 @@ struct NonlinearFactorCallback : NonlinearFactor, wrapper } }; -void exportNonlinearFactor(){ +// Similarly for NoiseModelFactor: +struct NoiseModelFactorWrap : NoiseModelFactor, wrapper { + // NOTE(frank): Add all these again as I can't figure out how to derive + double error (const Values & values) const { + return this->get_override("error")(values); + } + size_t dim () const { + return this->get_override("dim")(); + } + boost::shared_ptr linearize(const Values & values) const { + return this->get_override("linearize")(values); + } + Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + return this->get_override("unwhitenedError")(x, H); + } +}; - class_("NonlinearFactor") - ; +void exportNonlinearFactor() { + class_("NonlinearFactor") + .def("error", pure_virtual(&NonlinearFactor::error)) + .def("dim", pure_virtual(&NonlinearFactor::dim)) + .def("linearize", pure_virtual(&NonlinearFactor::linearize)); + register_ptr_to_python >(); -} \ No newline at end of file + class_("NoiseModelFactor") + .def("error", pure_virtual(&NoiseModelFactor::error)) + .def("dim", pure_virtual(&NoiseModelFactor::dim)) + .def("linearize", pure_virtual(&NoiseModelFactor::linearize)) + .def("unwhitenedError", pure_virtual(&NoiseModelFactor::unwhitenedError)); + register_ptr_to_python >(); +} diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp index 55929f3f1..6898133de 100644 --- a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -40,12 +40,15 @@ void exportNonlinearFactorGraph(){ typedef NonlinearFactorGraph::sharedFactor sharedFactor; void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back; + void (NonlinearFactorGraph::*push_back2)(const NonlinearFactorGraph&) = &NonlinearFactorGraph::push_back; void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add; class_("NonlinearFactorGraph", init<>()) .def("size",&NonlinearFactorGraph::size) .def("push_back", push_back1) + .def("push_back", push_back2) .def("add", add1) + .def("error", &NonlinearFactorGraph::error) .def("resize", &NonlinearFactorGraph::resize) .def("empty", &NonlinearFactorGraph::empty) .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index 0d1349972..9e4aad83f 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -47,6 +47,8 @@ void exportValues(){ void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; void (Values::*insert_bias) (Key, const Bias&) = &Values::insert; void (Values::*insert_navstate) (Key, const NavState&) = &Values::insert; + void (Values::*insert_vector) (Key, const gtsam::Vector&) = &Values::insert; + void (Values::*insert_vector2) (Key, const gtsam::Vector2&) = &Values::insert; void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert; class_("Values", init<>()) @@ -67,6 +69,8 @@ void exportValues(){ .def("insert", insert_pose3) .def("insert", insert_bias) .def("insert", insert_navstate) + .def("insert", insert_vector) + .def("insert", insert_vector2) .def("insert", insert_vector3) .def("atPoint2", &Values::at) .def("atRot2", &Values::at) @@ -76,6 +80,8 @@ void exportValues(){ .def("atPose3", &Values::at) .def("atConstantBias", &Values::at) .def("atNavState", &Values::at) + .def("atVector", &Values::at) + .def("atVector2", &Values::at) .def("atVector3", &Values::at) .def("exists", exists1) .def("keys", &Values::keys) diff --git a/python/setup.py.in b/python/setup.py.in index d3b5fcde4..8b2de7352 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -11,5 +11,5 @@ setup(name='gtsam', package_dir={ '': '${CMAKE_CURRENT_SOURCE_DIR}' }, packages=['gtsam', 'gtsam_utils', 'gtsam_examples', 'gtsam_tests'], #package_data={'gtsam' : ['_libgtsam_python.so']}, # location of .so file is relative to package_dir - data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_libgtsam_python.so'])], # location of .so file relative to setup.py + data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_gtsampy.so'])], # location of .so file relative to setup.py ) diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 0012f5f45..3245652be 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -140,7 +140,7 @@ namespace simulated2D { /// Return error and optional derivative Vector evaluateError(const Pose& x, boost::optional H = boost::none) const { - return (prior(x, H) - measured_).vector(); + return (prior(x, H) - measured_); } virtual ~GenericPrior() {} @@ -186,7 +186,7 @@ namespace simulated2D { Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (odo(x1, x2, H1, H2) - measured_).vector(); + return (odo(x1, x2, H1, H2) - measured_); } virtual ~GenericOdometry() {} @@ -233,7 +233,7 @@ namespace simulated2D { Vector evaluateError(const Pose& x1, const Landmark& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - measured_).vector(); + return (mea(x1, x2, H1, H2) - measured_); } virtual ~GenericMeasurement() {} diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index ccc734cfd..7d399dc02 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -111,7 +111,7 @@ namespace simulated2D { * @return a scalar distance between values */ template - double range_trait(const T1& a, const T2& b) { return a.dist(b); } + double range_trait(const T1& a, const T2& b) { return distance2(a, b); } /** * Binary inequality constraint forcing the range between points diff --git a/tests/smallExample.h b/tests/smallExample.h index 215655593..d3a69b0bd 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -180,7 +180,7 @@ inline boost::shared_ptr sharedNonlinearFactorGraph( new NonlinearFactorGraph); // prior on x1 - Point2 mu; + Point2 mu(0,0); shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1))); nlfg->push_back(f1); @@ -337,7 +337,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { if (A) *A = H(x); - return (h(x) - z_).vector(); + return (h(x) - z_); } }; @@ -593,11 +593,11 @@ inline boost::tuple planarGraph(size_t N) { Values zeros; for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) - zeros.insert(key(x, y), Point2()); + zeros.insert(key(x, y), Point2(0,0)); VectorValues xtrue; for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) - xtrue.insert(key(x, y), Point2((double)x, (double)y).vector()); + xtrue.insert(key(x, y), Point2((double)x, (double)y)); // linearize around zero boost::shared_ptr gfg = nlfg.linearize(zeros); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index b0b748d95..a5d1a195c 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -181,7 +181,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { /* ************************************************************************* */ TEST( testBoundingConstraint, MaxDistance_basics) { gtsam::Key key1 = 1, key2 = 2; - Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); + Point2 pt1(0,0), pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu); EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol); EXPECT(!rangeBound.isGreaterThan()); @@ -220,7 +220,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { /* ************************************************************************* */ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { - Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); + Point2 pt1(0,0), pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); Symbol x1('x',1), x2('x',2); NonlinearFactorGraph graph; @@ -246,7 +246,7 @@ TEST( testBoundingConstraint, avoid_demo) { Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1); double radius = 1.0; - Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); + Point2 x1_pt(0,0), x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); Point2 odo(2.0, 0.0); NonlinearFactorGraph graph; diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index a2bb57e1c..954afa220 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -139,7 +139,7 @@ TEST(DoglegOptimizer, Iterate) { VectorValues dx_u = gbn.optimizeGradientSearch(); VectorValues dx_n = gbn.optimize(); DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config)); - Delta = result.Delta; + Delta = result.delta; EXPECT(result.f_error < fg.error(config)); // Check that error decreases Values newConfig(config.retract(result.dx_d)); config = newConfig; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 4fda27cdb..b6b196acc 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -193,7 +193,7 @@ TEST(ExpressionFactor, Binary) { internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; Point2 value = binary.traceExecution(values, trace, traceStorage); - EXPECT(assert_equal(Point2(),value, 1e-9)); + EXPECT(assert_equal(Point2(0,0),value, 1e-9)); // trace.print(); // Expected Jacobians @@ -248,7 +248,7 @@ TEST(ExpressionFactor, Shallow) { internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); - EXPECT(assert_equal(Point2(),value, 1e-9)); + EXPECT(assert_equal(Point2(0,0),value, 1e-9)); // trace.print(); // Expected Jacobians diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 00ab4a16c..b3e8a3449 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -226,7 +226,7 @@ public: *H2 = Matrix::Identity(dim(),dim()); // Return the error between the prediction and the supplied value of p2 - return (p2 - prediction).vector(); + return (p2 - prediction); } }; @@ -400,7 +400,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { ExtendedKalmanFilter ekf(X(0), x_initial, P_initial); // Enter Predict-Update Loop - Point2 x_predict, x_update; + Point2 x_predict(0,0), x_update(0,0); for(unsigned int i = 0; i < 10; ++i){ // Create motion factor NonlinearMotionModel motionFactor(X(i), X(i+1)); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index e3c17fa3f..8b2aa3ae7 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -26,7 +26,6 @@ #include -#include #include // for operator += using namespace boost::assign; @@ -220,7 +219,7 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) // Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); // Permutation toFrontInverse(*toFront.inverse()); // varIndex.permute(toFront); -// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { +// for(const GaussianFactor::shared_ptr& factor: marginal) { // factor->permuteWithInverse(toFrontInverse); } // GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex); // actual.permuteWithInverse(toFront); diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 10de57435..eec62ff9c 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -26,7 +26,6 @@ #include -#include #include #include // for operator += #include // for operator += @@ -585,7 +584,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { GaussianBayesTree actBT = *lfg.eliminateMultifrontal(); // Check that all sigmas in an unconstrained bayes tree are set to one - BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes() | br::map_values) { + for(const GaussianBayesTree::sharedClique& clique: actBT.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); //size_t dim = conditional->rows(); //EXPECT(assert_equal(gtsam::Vector::Ones(dim), conditional->get_model()->sigmas(), tol)); diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 7f53406c8..07323e0fc 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -22,7 +22,6 @@ #include #include -#include #include // for operator += using namespace boost::assign; #include @@ -46,7 +45,7 @@ TEST( ISAM, iSAM_smoother ) // run iSAM for every factor GaussianISAM actual; - BOOST_FOREACH(boost::shared_ptr factor, smoother) { + for(boost::shared_ptr factor: smoother) { GaussianFactorGraph factorGraph; factorGraph.push_back(factor); actual.update(factorGraph); @@ -56,7 +55,7 @@ TEST( ISAM, iSAM_smoother ) GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering); // Verify sigmas in the bayes tree - BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, expected.nodes() | br::map_values) { + for(const GaussianBayesTree::sharedClique& clique: expected.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); EXPECT(!conditional->get_model()); } diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index d358caa35..f56b458be 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include using namespace boost::assign; @@ -212,7 +211,7 @@ struct ConsistencyVisitor if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end()) consistent = false; } - BOOST_FOREACH(Key j, node->conditional()->frontals()) + for(Key j: node->conditional()->frontals()) { if(isam.nodes().at(j).get() != node.get()) consistent = false; @@ -251,7 +250,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c // Check gradient at each node bool nodeGradientsOk = true; typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + for(const sharedClique& clique: isam.nodes() | br::map_values) { // Compute expected gradient GaussianFactorGraph jfg; jfg += clique->conditional(); @@ -483,7 +482,7 @@ TEST(ISAM2, swapFactors) // Check gradient at each node typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + for(const sharedClique& clique: isam.nodes() | br::map_values) { // Compute expected gradient GaussianFactorGraph jfg; jfg += clique->conditional(); @@ -608,7 +607,7 @@ TEST(ISAM2, constrained_ordering) // Check gradient at each node typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + for(const sharedClique& clique: isam.nodes() | br::map_values) { // Compute expected gradient GaussianFactorGraph jfg; jfg += clique->conditional(); @@ -650,7 +649,7 @@ namespace { bool checkMarginalizeLeaves(ISAM2& isam, const FastList& leafKeys) { Matrix expectedAugmentedHessian, expected3AugmentedHessian; vector toKeep; - BOOST_FOREACH(Key j, isam.getDelta() | br::map_keys) + for(Key j: isam.getDelta() | br::map_keys) if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end()) toKeep.push_back(j); diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index aea18c90d..95caaaf9c 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -29,7 +29,6 @@ #include #include #include -#include #include #include #include @@ -51,8 +50,8 @@ TEST(PinholeCamera, BAL) { NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) - graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + for (const SfM_Measurement& m: db.tracks[j].measurements) + graph.emplace_shared(m.second, unit2, m.first, P(j)); } Values initial = initialCamerasAndPointsEstimate(db); diff --git a/tests/testLie.cpp b/tests/testLie.cpp index c153adf5f..2d8a0b975 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -40,7 +40,7 @@ template<> struct traits : internal::LieGroupTraits { << m.second.theta() << ")" << endl; } static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { - return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol); + return traits::Equals(m1.first, m2.first, tol) && m1.second.equals(m2.second, tol); } }; } @@ -102,6 +102,33 @@ TEST( testProduct, inverse ) { EXPECT(assert_equal(numericH1, actH1, tol)); } +/* ************************************************************************* */ +Product expmap_proxy(const Vector5& vec) { + return Product::Expmap(vec); +} +TEST( testProduct, Expmap ) { + Vector5 vec; + vec << 1, 2, 0.1, 0.2, 0.3; + + Matrix actH; + Product::Expmap(vec, actH); + Matrix numericH = numericalDerivative11(expmap_proxy, vec); + EXPECT(assert_equal(numericH, actH, tol)); +} + +/* ************************************************************************* */ +Vector5 logmap_proxy(const Product& p) { + return Product::Logmap(p); +} +TEST( testProduct, Logmap ) { + Product state(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH; + Product::Logmap(state, actH); + Matrix numericH = numericalDerivative11(logmap_proxy, state); + EXPECT(assert_equal(numericH, actH, tol)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 65d26eb98..286e3ff5e 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -78,7 +78,7 @@ TEST(Manifold, Identity) { EXPECT_DOUBLES_EQUAL(0.0, traits::Identity(), 0.0); EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::Identity()))); EXPECT(assert_equal(Pose3(), traits::Identity())); - EXPECT(assert_equal(Point2(), traits::Identity())); + EXPECT(assert_equal(Point2(0,0), traits::Identity())); } //****************************************************************************** @@ -166,7 +166,7 @@ template<> struct traits : internal::ManifoldTraits TEST(Manifold, ProductManifold) { BOOST_CONCEPT_ASSERT((IsManifold)); - MyPoint2Pair pair1; + MyPoint2Pair pair1(Point2(0,0),Point2(0,0)); Vector4 d; d << 1,2,3,4; MyPoint2Pair expected(Point2(1,2),Point2(3,4)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 86080b673..95843e5ab 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -417,7 +417,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { graph.push_back(factor); Values initValues; - initValues.insert(key1, Point2()); + initValues.insert(key1, Point2(0,0)); initValues.insert(key2, badPt); Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); @@ -454,7 +454,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Values initialEstimate; initialEstimate.insert(x1, pt_x1); - initialEstimate.insert(x2, Point2()); + initialEstimate.insert(x2, Point2(0,0)); initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index ee60f9714..9556a8018 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -9,7 +9,7 @@ * -------------------------------------------------------------------------- */ -/** +/** * @file testNonlinearFactorGraph.cpp * @brief Unit tests for Non-Linear Factor NonlinearFactorGraph * @brief testNonlinearFactorGraph @@ -107,9 +107,9 @@ TEST( NonlinearFactorGraph, linearize ) { NonlinearFactorGraph fg = createNonlinearFactorGraph(); Values initial = createNoisyValues(); - GaussianFactorGraph linearized = *fg.linearize(initial); + GaussianFactorGraph linearFG = *fg.linearize(initial); GaussianFactorGraph expected = createGaussianFactorGraph(); - CHECK(assert_equal(expected,linearized)); // Needs correct linearizations + CHECK(assert_equal(expected,linearFG)); // Needs correct linearizations } /* ************************************************************************* */ @@ -165,6 +165,38 @@ TEST( NonlinearFactorGraph, symbolic ) EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(NonlinearFactorGraph, UpdateCholesky) { + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + Values initial = createNoisyValues(); + + // solve conventionally + GaussianFactorGraph linearFG = *fg.linearize(initial); + auto delta = linearFG.optimizeDensely(); + auto expected = initial.retract(delta); + + // solve with new method + EXPECT(assert_equal(expected, fg.updateCholesky(initial))); + + // solve with Ordering + Ordering ordering; + ordering += L(1), X(2), X(1); + EXPECT(assert_equal(expected, fg.updateCholesky(initial, ordering))); + + // solve with new method, heavily damped + auto dampen = [](const HessianFactor::shared_ptr& hessianFactor) { + auto iterator = hessianFactor->begin(); + for (; iterator != hessianFactor->end(); iterator++) { + const auto index = std::distance(hessianFactor->begin(), iterator); + auto block = hessianFactor->info().diagonalBlock(index); + for (int j = 0; j < block.rows(); j++) { + block(j, j) += 1e9; + } + } + }; + EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 3c49d54af..617a8cc1c 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -117,9 +117,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { new_factors += PriorFactor(lm3, landmark3, model2); // Initialize to origin - new_init.insert(lm1, Point2()); - new_init.insert(lm2, Point2()); - new_init.insert(lm3, Point2()); + new_init.insert(lm1, Point2(0,0)); + new_init.insert(lm2, Point2(0,0)); + new_init.insert(lm3, Point2(0,0)); } isamChol.update(new_factors, new_init); @@ -194,9 +194,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { new_factors += PriorFactor(lm3, landmark3, model2); // Initialize to origin - new_init.insert(lm1, Point2()); - new_init.insert(lm2, Point2()); - new_init.insert(lm3, Point2()); + new_init.insert(lm1, Point2(0,0)); + new_init.insert(lm2, Point2(0,0)); + new_init.insert(lm3, Point2(0,0)); } // Reconnect with observation later diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 7a22abc67..9ddbbb1b2 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -32,9 +32,11 @@ #include +#include #include #include // for operator += using namespace boost::assign; +using boost::adaptors::map_values; #include #include @@ -263,13 +265,6 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { // Try LM and Dogleg LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); - // params.setVerbosityLM("TRYDELTA"); - // params.setVerbosity("TERMINATION"); - params.lambdaUpperBound = 1e9; -// params.relativeErrorTol = 0; -// params.absoluteErrorTol = 0; - //params.lambdaInitial = 10; - { LevenbergMarquardtOptimizer optimizer(fg, init, params); @@ -297,9 +292,11 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { // test the diagonal GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); - GaussianFactorGraph damped = *optimizer.buildDampedSystem(*linear); - VectorValues d = linear->hessianDiagonal(), // - expectedDiagonal = d + params.lambdaInitial * d; + VectorValues d = linear->hessianDiagonal(); + VectorValues sqrtHessianDiagonal = d; + for (Vector& v : sqrtHessianDiagonal | map_values) v = v.cwiseSqrt(); + GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear, sqrtHessianDiagonal); + VectorValues expectedDiagonal = d + params.lambdaInitial * d; EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal())); // test convergence (does not!) @@ -311,7 +308,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { EXPECT(assert_equal(expectedGradient,linear->gradientAtZero())); // Check that the gradient is zero for damped system (it is not!) - damped = *optimizer.buildDampedSystem(*linear); + damped = optimizer.buildDampedSystem(*linear, sqrtHessianDiagonal); VectorValues actualGradient = damped.gradientAtZero(); EXPECT(assert_equal(expectedGradient,actualGradient)); @@ -364,8 +361,9 @@ TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { expected.insert(1, Pose2(1,0,M_PI/2)); expected.insert(2, Pose2(1,1,M_PI)); + LevenbergMarquardtParams params; EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize())); - EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init).optimize())); + EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize())); EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); } @@ -463,7 +461,7 @@ TEST( NonlinearOptimizer, logfile ) } /* ************************************************************************* */ -// Minimal traits example +//// Minimal traits example struct MyType : public Vector3 { using Vector3::Vector3; }; @@ -471,11 +469,13 @@ struct MyType : public Vector3 { namespace gtsam { template <> struct traits { - static bool Equals(const MyType&, const MyType&, double tol) {return true;} + static bool Equals(const MyType& a, const MyType& b, double tol) { + return (a - b).array().abs().maxCoeff() < tol; + } static void Print(const MyType&, const string&) {} - static int GetDimension(const MyType&) { return 3;} - static MyType Retract(const MyType&, const Vector3&) {return MyType();} - static Vector3 Local(const MyType&, const MyType&) {return Vector3();} + static int GetDimension(const MyType&) { return 3; } + static MyType Retract(const MyType& a, const Vector3& b) { return a + b; } + static Vector3 Local(const MyType& a, const MyType& b) { return b - a; } }; } diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 6a49f66d3..accf9a65e 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -27,7 +27,6 @@ #include #include -#include #include #include using namespace boost::assign; @@ -57,7 +56,7 @@ TEST( SubgraphPreconditioner, planarOrdering ) { /** unnormalized error */ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) + for(const GaussianFactor::shared_ptr& factor: fg) total_error += factor->error(x); return total_error; } diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index f5d0384f2..aeeed1b9f 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -28,7 +28,6 @@ #include #include -#include #include #include using namespace boost::assign; @@ -41,7 +40,7 @@ using namespace example; /** unnormalized error */ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) + for(const GaussianFactor::shared_ptr& factor: fg) total_error += factor->error(x); return total_error; } diff --git a/timing/DummyFactor.h b/timing/DummyFactor.h index 08e9d8f4b..5e2664d26 100644 --- a/timing/DummyFactor.h +++ b/timing/DummyFactor.h @@ -40,7 +40,7 @@ public: void multiplyHessian(double alpha, const VectorValues& x, VectorValues& y) const { - BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) { + for(const KeyMatrix2D& Fi: this->Fblocks_) { static const Vector empty; Key key = Fi.first; std::pair it = y.tryInsert(key, empty); diff --git a/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp index 3a9b5297a..8950c636b 100644 --- a/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -68,7 +68,7 @@ int main() { values.insert(2,Vector3(0,0,1)); typedef AdaptAutoDiff AdaptedSnavely; Expression expression(AdaptedSnavely(), Expression(1), Expression(2)); - f2 = boost::make_shared >(model, z.vector(), expression); + f2 = boost::make_shared >(model, z, expression); time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); return 0; diff --git a/timing/timeBatch.cpp b/timing/timeBatch.cpp index a1e5f359b..4ed1a4555 100644 --- a/timing/timeBatch.cpp +++ b/timing/timeBatch.cpp @@ -57,7 +57,7 @@ int main(int argc, char *argv[]) { // Compute marginals Marginals marginals(graph, optimizer.values()); int i=0; - BOOST_FOREACH(Key key, initial.keys()) { + for(Key key: initial.keys()) { gttic_(marginalInformation); Matrix info = marginals.marginalInformation(key); gttoc_(marginalInformation); diff --git a/timing/timeCholesky.cpp b/timing/timeCholesky.cpp new file mode 100644 index 000000000..4dd349593 --- /dev/null +++ b/timing/timeCholesky.cpp @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeCholesky.cpp + * @brief time Cholesky factorization + * @author Frank Dellaert + * @date March 4, 2016 + */ + +#include + +#include +#include +#include // std::setprecision + +using namespace std; +using namespace gtsam; + +//#define TERNARY + +int main() { + + Matrix top = (Matrix(7,7) << + 4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063, + 0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914, + 0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992, + 0., 0., 0., 1.8786, 1.0535, 1.4250, 1.3347, + 0., 0., 0., 0., 3.0788, 2.6283, 2.3791, + 0., 0., 0., 0., 0., 2.9227, 2.4056, + 0., 0., 0., 0., 0., 0., 2.5776).finished(); + + Matrix ABC(100,100); + ABC.topLeftCorner<7,7>() = top; + cout << setprecision(3); + + size_t n = 100000; + for (size_t nFrontal = 1; nFrontal <= 7; nFrontal++) { + auto timeLog = clock(); + for (size_t i = 0; i < n; i++) { + Matrix RSL(ABC); + choleskyPartial(RSL, nFrontal); + } + auto timeLog2 = clock(); + auto seconds = (double)(timeLog2 - timeLog) / CLOCKS_PER_SEC; + cout << "partialCholesky " << nFrontal << ": "; + auto ms = ((double)seconds * 1000000 / n); + cout << ms << " ms, " << ms/nFrontal << " ms/dim" << endl; + } + + return 0; +} diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp index a3157729e..cd11f6360 100644 --- a/timing/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -16,7 +16,6 @@ */ #include -#include #include // for operator += in Ordering #include #include @@ -82,7 +81,7 @@ double timePlanarSmootherEliminate(int N, bool old = true) { // // create an internal ordering to render Ab // Ordering render; // render += key; -// BOOST_FOREACH(const Symbol& k, joint_factor->keys()) +// for(const Symbol& k: joint_factor->keys()) // if (k != key) render += k; // // Matrix Ab = joint_factor->matrix_augmented(render,false); diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index b5dd37516..61e129699 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -60,7 +61,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c // the factor graph already includes a factor for the prior/equality constraint. // double dof = graph.size() - config.size(); int graph_dim = 0; - BOOST_FOREACH(const boost::shared_ptr& nlf, graph) { + for(const boost::shared_ptr& nlf: graph) { graph_dim += nlf->dim(); } double dof = graph_dim - config.dim(); // kaess: changed to dim @@ -225,9 +226,9 @@ int main(int argc, char *argv[]) { try { Marginals marginals(graph, values); int i=0; - BOOST_REVERSE_FOREACH(Key key1, values.keys()) { + for (Key key1: boost::adaptors::reverse(values.keys())) { int j=0; - BOOST_REVERSE_FOREACH(Key key2, values.keys()) { + for (Key key2: boost::adaptors::reverse(values.keys())) { if(i != j) { gttic_(jointMarginalInformation); std::vector keys(2); @@ -246,7 +247,7 @@ int main(int argc, char *argv[]) { break; } tictoc_print_(); - BOOST_FOREACH(Key key, values.keys()) { + for(Key key: values.keys()) { gttic_(marginalInformation); Matrix info = marginals.marginalInformation(key); gttoc_(marginalInformation); diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index 3a4da89b5..d0b6b263c 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -45,7 +45,7 @@ int main(int argc, char *argv[]) { Sampler sampler(42u); Values::ConstFiltered poses = solution->filter(); SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, poses) + for(const Values::ConstFiltered::KeyValuePair& it: poses) initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); // Add prior on the pose having index (key) = 0 diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index a2d11a756..a2010891b 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include @@ -173,29 +172,29 @@ int main(int argc, char* argv[]) { // Do a few initial assignments to let any cache effects stabilize for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { mat(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); } for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { mat(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); } basicTime = tim.elapsed(); cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl; for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { full(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rng(); } fullTime = tim.elapsed(); cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl; for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { top(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rng(); } topTime = tim.elapsed(); cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl; for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { block(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rng(); } blockTime = tim.elapsed(); cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl; diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index 458f88db1..1578bb0a8 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -64,7 +64,7 @@ int main() long timeLog = clock(); Point2 measurement(0,0); for(int i = 0; i < n; i++) - measurement.localCoordinates(camera.project(point1)); + camera.project(point1)-measurement; long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl; diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 6308a1d61..be1104b1a 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -23,7 +23,6 @@ #include #include -#include using namespace std; using namespace gtsam; @@ -38,18 +37,18 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; - graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j))); + graph.emplace_shared(z, gNoiseModel, C(i), P(j)); } } Values initial; size_t i = 0, j = 0; - BOOST_FOREACH (const SfM_Camera& camera, db.cameras) + for (const SfM_Camera& camera: db.cameras) initial.insert(C(i++), camera); - BOOST_FOREACH (const SfM_Track& track, db.tracks) + for (const SfM_Track& track: db.tracks) initial.insert(P(j++), track.p); return optimize(db, graph, initial); diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 1642af7e8..1ca9f82d2 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -67,6 +67,7 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph, // Set parameters to be similar to ceres LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); +// params.setLinearSolverType("SEQUENTIAL_CHOLESKY"); // params.setVerbosityLM("SUMMARY"); if (gUseSchur) { diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 4545abc21..eb1a46606 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include #include @@ -46,7 +45,7 @@ int main(int argc, char* argv[]) { // Build graph NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; Expression camera_(C(i)); @@ -59,15 +58,15 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; - BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + for (const SfM_Camera& camera: db.cameras) { // readBAL converts to GTSAM format, so we need to convert back ! Pose3 openGLpose = gtsam2openGL(camera.pose()); Vector9 v9; - v9 << Pose3::Logmap(openGLpose), camera.calibration().vector(); + v9 << Pose3::Logmap(openGLpose), camera.calibration(); initial.insert(C(i++), v9); } - BOOST_FOREACH (const SfM_Track& track, db.tracks) { - Vector3 v3 = track.p.vector(); + for (const SfM_Track& track: db.tracks) { + Vector3 v3 = track.p; initial.insert(P(j++), v3); } diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp index 32fae4ac2..33680e813 100644 --- a/timing/timeSFMBALcamTnav.cpp +++ b/timing/timeSFMBALcamTnav.cpp @@ -23,7 +23,6 @@ #include #include -#include using namespace std; using namespace gtsam; @@ -35,7 +34,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; Pose3_ camTnav_(C(i)); @@ -50,12 +49,12 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; - BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + for (const SfM_Camera& camera: db.cameras) { initial.insert(C(i), camera.pose().inverse()); // inverse !!! initial.insert(K(i), camera.calibration()); i += 1; } - BOOST_FOREACH (const SfM_Track& track, db.tracks) + for (const SfM_Track& track: db.tracks) initial.insert(P(j++), track.p); bool separateCalibration = true; diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index 0d0eb4f65..dad1edd4e 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -23,7 +23,6 @@ #include #include -#include using namespace std; using namespace gtsam; @@ -36,7 +35,7 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { Point3_ nav_point_(P(j)); - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; Pose3_ navTcam_(C(i)); @@ -50,12 +49,12 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; - BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + for (const SfM_Camera& camera: db.cameras) { initial.insert(C(i), camera.pose()); initial.insert(K(i), camera.calibration()); i += 1; } - BOOST_FOREACH (const SfM_Track& track, db.tracks) + for (const SfM_Track& track: db.tracks) initial.insert(P(j++), track.p); bool separateCalibration = true; diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index 9be55f831..e64c34340 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -150,7 +150,7 @@ int main(void) { ms += m; //for (size_t m=10;m<=100;m+=10) ms += m; // loop over number of images - BOOST_FOREACH(size_t m,ms) + for(size_t m: ms) timeAll >(m, NUM_ITERATIONS); } diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index c4196f414..7a4413ac7 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -93,7 +93,7 @@ int main(int argc, char *argv[]) { // Write times ofstream timesFile("times.txt"); - BOOST_FOREACH(double t, times) { + for(double t: times) { timesFile << t << "\n"; } return 0; diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 848998eb0..01da3a756 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -18,8 +18,6 @@ #include "Argument.h" -#include -#include #include #include @@ -40,7 +38,7 @@ Argument Argument::expandTemplate(const TemplateSubstitution& ts) const { ArgumentList ArgumentList::expandTemplate( const TemplateSubstitution& ts) const { ArgumentList instArgList; - BOOST_FOREACH(const Argument& arg, *this) { + for(const Argument& arg: *this) { Argument instArg = arg.expandTemplate(ts); instArgList.push_back(instArg); } @@ -50,7 +48,7 @@ ArgumentList ArgumentList::expandTemplate( /* ************************************************************************* */ string Argument::matlabClass(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, type.namespaces()) + for(const string& ns: type.namespaces()) result += ns + delim; if (type.name() == "string" || type.name() == "unsigned char" || type.name() == "char") @@ -77,16 +75,22 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { string cppType = type.qualifiedName("::"); string matlabUniqueType = type.qualifiedName(); + bool isNotScalar = !Argument::isScalar(); + + // We cannot handle scalar non const references + if (!isNotScalar && is_ref && !is_const) { + throw std::runtime_error("Cannot unwrap a scalar non-const reference"); + } if (is_ptr && type.category != Qualified::EIGEN) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; - else if (is_ref && type.category != Qualified::EIGEN) + else if (is_ref && isNotScalar && type.category != Qualified::EIGEN) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; else - // Not a pointer or a reference: emit an "unwrap" call + // Not a pointer, or a reference to a scalar type. Therefore, emit an "unwrap" call // unwrap is specified in matlab.h as a series of template specializations // that know how to unpack the expected MATLAB object // example: double tol = unwrap< double >(in[2]); @@ -94,7 +98,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if( (is_ptr || is_ref) && type.category != Qualified::EIGEN) + if( (is_ptr || is_ref) && isNotScalar && type.category != Qualified::EIGEN) file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } @@ -110,7 +114,7 @@ void Argument::proxy_check(FileWriter& proxyFile, const string& s) const { string ArgumentList::types() const { string str; bool first = true; - BOOST_FOREACH(Argument arg, *this) { + for(Argument arg: *this) { if (!first) str += ","; str += arg.type.name(); @@ -124,8 +128,8 @@ string ArgumentList::signature() const { string sig; bool cap = false; - BOOST_FOREACH(Argument arg, *this) { - BOOST_FOREACH(char ch, arg.type.name()) + for(Argument arg: *this) { + for(char ch: arg.type.name()) if (isupper(ch)) { sig += ch; //If there is a capital letter, we don't want to read it below @@ -144,7 +148,7 @@ string ArgumentList::signature() const { string ArgumentList::names() const { string str; bool first = true; - BOOST_FOREACH(Argument arg, *this) { + for(Argument arg: *this) { if (!first) str += ","; str += arg.name; @@ -155,7 +159,7 @@ string ArgumentList::names() const { /* ************************************************************************* */ bool ArgumentList::allScalar() const { - BOOST_FOREACH(Argument arg, *this) + for(Argument arg: *this) if (!arg.isScalar()) return false; return true; @@ -164,7 +168,7 @@ bool ArgumentList::allScalar() const { /* ************************************************************************* */ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { int index = start; - BOOST_FOREACH(Argument arg, *this) { + for(Argument arg: *this) { stringstream buf; buf << "in[" << index << "]"; arg.matlab_unwrap(file, buf.str()); @@ -176,7 +180,7 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { file.oss << name << "("; bool first = true; - BOOST_FOREACH(Argument arg, *this) { + for(Argument arg: *this) { if (!first) file.oss << ", "; file.oss << arg.type.name() << " " << arg.name; diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 03915a662..04d471b39 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -1,6 +1,6 @@ # Build/install Wrap -set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_REGEX_LIBRARY}) +set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY}) # Allow for disabling serialization to handle errors related to Clang's linker option(GTSAM_WRAP_SERIALIZATION "If enabled, allows for wrapped objects to be saved via boost.serialization" ON) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index d7a3b6ee4..3a12290eb 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -20,7 +20,6 @@ #include "utilities.h" #include "Argument.h" -#include #include #include #include @@ -157,7 +156,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods_) { + for(const Methods::value_type& name_m: methods_) { const Method& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); @@ -172,7 +171,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " methods(Static = true)\n"; // Static methods - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { + for(const StaticMethods::value_type& name_m: static_methods) { const StaticMethod& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); @@ -301,7 +300,7 @@ Class Class::expandTemplate(const TemplateSubstitution& ts) const { vector Class::expandTemplate(Str templateArg, const vector& instantiations) const { vector result; - BOOST_FOREACH(const Qualified& instName, instantiations) { + for(const Qualified& instName: instantiations) { Qualified expandedClass = (Qualified) (*this); expandedClass.expand(instName.name()); const TemplateSubstitution ts(templateArg, instName, expandedClass); @@ -319,7 +318,7 @@ vector Class::expandTemplate(Str templateArg, vector Class::expandTemplate(Str templateArg, const vector& integers) const { vector result; - BOOST_FOREACH(int i, integers) { + for(int i: integers) { Qualified expandedClass = (Qualified) (*this); stringstream ss; ss << i; string instName = ss.str(); @@ -342,7 +341,7 @@ void Class::addMethod(bool verbose, bool is_const, Str methodName, if (tmplate.valid()) { // Create method to expand // For all values of the template argument, create a new method - BOOST_FOREACH(const Qualified& instName, tmplate.argValues()) { + for(const Qualified& instName: tmplate.argValues()) { const TemplateSubstitution ts(tmplate.argName(), instName, *this); // substitute template in arguments ArgumentList expandedArgs = argumentList.expandTemplate(ts); @@ -413,7 +412,7 @@ void Class::appendInheritedMethods(const Class& cls, if (cls.parentClass) { // Find parent - BOOST_FOREACH(const Class& parent, classes) { + for(const Class& parent: classes) { // We found a parent class for our parent, TODO improve ! if (parent.name() == cls.parentClass->name()) { methods_.insert(parent.methods_.begin(), parent.methods_.end()); @@ -426,7 +425,7 @@ void Class::appendInheritedMethods(const Class& cls, /* ************************************************************************* */ string Class::getTypedef() const { string result; - BOOST_FOREACH(Str namesp, namespaces()) { + for(Str namesp: namespaces()) { result += ("namespace " + namesp + " { "); } result += ("typedef " + typedefName + " " + name() + ";"); @@ -446,12 +445,12 @@ void Class::comment_fragment(FileWriter& proxyFile) const { if (!methods_.empty()) proxyFile.oss << "%\n%-------Methods-------\n"; - BOOST_FOREACH(const Methods::value_type& name_m, methods_) + for(const Methods::value_type& name_m: methods_) name_m.second.comment_fragment(proxyFile); if (!static_methods.empty()) proxyFile.oss << "%\n%-------Static Methods-------\n"; - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) + for(const StaticMethods::value_type& name_m: static_methods) name_m.second.comment_fragment(proxyFile); if (hasSerialization) { @@ -653,9 +652,9 @@ string Class::getSerializationExport() const { void Class::python_wrapper(FileWriter& wrapperFile) const { wrapperFile.oss << "class_<" << name() << ">(\"" << name() << "\")\n"; constructor.python_wrapper(wrapperFile, name()); - BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values) + for(const StaticMethod& m: static_methods | boost::adaptors::map_values) m.python_wrapper(wrapperFile, name()); - BOOST_FOREACH(const Method& m, methods_ | boost::adaptors::map_values) + for(const Method& m: methods_ | boost::adaptors::map_values) m.python_wrapper(wrapperFile, name()); wrapperFile.oss << ";\n\n"; } diff --git a/wrap/Class.h b/wrap/Class.h index 2f7457f06..e69c38f41 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -39,7 +39,6 @@ namespace bl = boost::lambda; -#include #include #include @@ -145,9 +144,9 @@ public: friend std::ostream& operator<<(std::ostream& os, const Class& cls) { os << "class " << cls.name() << "{\n"; os << cls.constructor << ";\n"; - BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values) + for(const StaticMethod& m: cls.static_methods | boost::adaptors::map_values) os << m << ";\n"; - BOOST_FOREACH(const Method& m, cls.methods_ | boost::adaptors::map_values) + for(const Method& m: cls.methods_ | boost::adaptors::map_values) os << m << ";\n"; os << "};" << std::endl; return os; diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 35cc0fa53..77d831e0a 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -20,7 +20,6 @@ #include #include -#include #include #include "utilities.h" diff --git a/wrap/Deconstructor.cpp b/wrap/Deconstructor.cpp index 9fe9aecc2..9e6495602 100644 --- a/wrap/Deconstructor.cpp +++ b/wrap/Deconstructor.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #include "utilities.h" diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index 30e27764b..80d974d88 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -44,7 +44,7 @@ public: void verifyReturnTypes(const std::vector& validtypes, const std::string& s) const { - BOOST_FOREACH(const ReturnValue& retval, returnVals_) { + for(const ReturnValue& retval: returnVals_) { retval.type1.verify(validtypes, s); if (retval.isPair) retval.type2.verify(validtypes, s); @@ -55,7 +55,7 @@ public: std::vector expandReturnValuesTemplate( const TemplateSubstitution& ts) const { std::vector result; - BOOST_FOREACH(const ReturnValue& retVal, returnVals_) { + for(const ReturnValue& retVal: returnVals_) { ReturnValue instRetVal = retVal.expandTemplate(ts); result.push_back(instRetVal); } @@ -73,7 +73,7 @@ public: // emit a list of comments, one for each overload void usage_fragment(FileWriter& proxyFile, const std::string& name) const { unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists_) { + for(ArgumentList argList: argLists_) { argList.emit_prototype(proxyFile, name); if (argLCount != nrOverloads() - 1) proxyFile.oss << ", "; @@ -87,7 +87,7 @@ public: // emit a list of comments, one for each overload void comment_fragment(FileWriter& proxyFile, const std::string& name) const { size_t i = 0; - BOOST_FOREACH(ArgumentList argList, argLists_) { + for(ArgumentList argList: argLists_) { proxyFile.oss << "%"; argList.emit_prototype(proxyFile, name); proxyFile.oss << " : returns " << returnVals_[i++].return_type(false) @@ -125,7 +125,7 @@ template inline void verifyReturnTypes(const std::vector& validTypes, const std::map& vt) { typedef typename std::map::value_type NamedMethod; - BOOST_FOREACH(const NamedMethod& namedMethod, vt) + for(const NamedMethod& namedMethod: vt) namedMethod.second.verifyReturnTypes(validTypes); } diff --git a/wrap/Function.cpp b/wrap/Function.cpp index 7e4ded040..8da667fae 100644 --- a/wrap/Function.cpp +++ b/wrap/Function.cpp @@ -18,7 +18,6 @@ #include "Function.h" #include "utilities.h" -#include #include #include diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 013ef6d28..3f667e2c9 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -8,7 +8,6 @@ #include "GlobalFunction.h" #include "utilities.h" -#include #include namespace wrap { @@ -44,7 +43,7 @@ void GlobalFunction::matlab_proxy(const string& toolboxPath, } size_t lastcheck = grouped_functions.size(); - BOOST_FOREACH(const GlobalFunctionMap::value_type& p, grouped_functions) { + for(const GlobalFunctionMap::value_type& p: grouped_functions) { p.second.generateSingleFunction(toolboxPath, wrapperName, typeAttributes, file, functionNames); if (--lastcheck != 0) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 2ad6e8259..f8c03b0c6 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -18,7 +18,6 @@ #include "Method.h" #include "utilities.h" -#include #include #include diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp index 9b5f7135f..84e6c67d9 100644 --- a/wrap/MethodBase.cpp +++ b/wrap/MethodBase.cpp @@ -19,7 +19,6 @@ #include "Method.h" #include "utilities.h" -#include #include #include diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 9d505a525..61d2a29e0 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -22,7 +22,6 @@ #include "TypeAttributesTable.h" #include "utilities.h" -#include #include #include @@ -58,7 +57,7 @@ static void handle_possible_template(vector& classes, const Class& cls, vector classInstantiations = (t.nrValues() > 0) ? cls.expandTemplate(arg, t.argValues()) : cls.expandTemplate(arg, t.intList()); - BOOST_FOREACH(const Class& c, classInstantiations) + for(const Class& c: classInstantiations) classes.push_back(c); } } @@ -153,7 +152,7 @@ void Module::parseMarkup(const std::string& data) { // parse forward declaration ForwardDeclaration fwDec0, fwDec; Rule forward_declaration_p = - !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) + !(str_p("virtual")[assign_a(fwDec.isVirtual, T)]) >> str_p("class") >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] >> ch_p(';') @@ -177,11 +176,11 @@ void Module::parseMarkup(const std::string& data) { } // Post-process classes for serialization markers - BOOST_FOREACH(Class& cls, classes) + for(Class& cls: classes) cls.erase_serialization(); // Explicitly add methods to the classes from parents so it shows in documentation - BOOST_FOREACH(Class& cls, classes) + for(Class& cls: classes) cls.appendInheritedMethods(cls, classes); // Expand templates - This is done first so that template instantiations are @@ -199,7 +198,7 @@ void Module::parseMarkup(const std::string& data) { verifyReturnTypes(validTypes, global_functions); hasSerialiable = false; - BOOST_FOREACH(const Class& cls, expandedClasses) + for(const Class& cls: expandedClasses) cls.verifyAll(validTypes,hasSerialiable); // Create type attributes table and check validity @@ -224,7 +223,6 @@ void Module::matlab_code(const string& toolboxPath) const { FileWriter wrapperFile(wrapperFileName, verbose, "//"); wrapperFile.oss << "#include \n"; wrapperFile.oss << "#include \n"; - wrapperFile.oss << "#include \n"; wrapperFile.oss << "\n"; // Include boost.serialization archive headers before other class headers @@ -239,14 +237,14 @@ void Module::matlab_code(const string& toolboxPath) const { // create typedef classes - we put this at the top of the wrap file so that // collectors and method arguments can use these typedefs - BOOST_FOREACH(const Class& cls, expandedClasses) + for(const Class& cls: expandedClasses) if(!cls.typedefName.empty()) wrapperFile.oss << cls.getTypedef() << "\n"; wrapperFile.oss << "\n"; // Generate boost.serialization export flags (needs typedefs from above) if (hasSerialiable) { - BOOST_FOREACH(const Class& cls, expandedClasses) + for(const Class& cls: expandedClasses) if(cls.isSerializable) wrapperFile.oss << cls.getSerializationExport() << "\n"; wrapperFile.oss << "\n"; @@ -261,11 +259,11 @@ void Module::matlab_code(const string& toolboxPath) const { vector functionNames; // Function names stored by index for switch // create proxy class and wrapper code - BOOST_FOREACH(const Class& cls, expandedClasses) + for(const Class& cls: expandedClasses) cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames); // create matlab files and wrapper code for global functions - BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions) + for(const GlobalFunctions::value_type& p: global_functions) p.second.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames); // finish wrapper file @@ -321,7 +319,7 @@ vector Module::ExpandTypedefInstantiations(const vector& classes, vector expandedClasses = classes; - BOOST_FOREACH(const TemplateInstantiationTypedef& inst, instantiations) { + for(const TemplateInstantiationTypedef& inst: instantiations) { // Add the new class to the list expandedClasses.push_back(inst.findAndExpand(classes)); } @@ -339,7 +337,7 @@ vector Module::ExpandTypedefInstantiations(const vector& classes, /* ************************************************************************* */ vector Module::GenerateValidTypes(const vector& classes, const vector forwardDeclarations) { vector validTypes; - BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDeclarations) { + for(const ForwardDeclaration& fwDec: forwardDeclarations) { validTypes.push_back(fwDec.name); } validTypes.push_back("void"); @@ -353,7 +351,7 @@ vector Module::GenerateValidTypes(const vector& classes, const ve validTypes.push_back("Vector"); validTypes.push_back("Matrix"); //Create a list of parsed classes for dependency checking - BOOST_FOREACH(const Class& cls, classes) { + for(const Class& cls: classes) { validTypes.push_back(cls.qualifiedName("::")); } @@ -363,7 +361,7 @@ vector Module::GenerateValidTypes(const vector& classes, const ve /* ************************************************************************* */ void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes) { // Generate all collectors - BOOST_FOREACH(const Class& cls, classes) { + for(const Class& cls: classes) { const string matlabUniqueName = cls.qualifiedName(), cppName = cls.qualifiedName("::"); wrapperFile.oss << "typedef std::set*> " @@ -379,7 +377,7 @@ void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::st " mstream mout;\n" // Send stdout to MATLAB console " std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n" " bool anyDeleted = false;\n"; - BOOST_FOREACH(const Class& cls, classes) { + for(const Class& cls: classes) { const string matlabUniqueName = cls.qualifiedName(); const string cppName = cls.qualifiedName("::"); const string collectorType = "Collector_" + matlabUniqueName; @@ -411,7 +409,7 @@ void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& modul " const mxArray *alreadyCreated = mexGetVariablePtr(\"global\", \"gtsam_" + moduleName + "_rttiRegistry_created\");\n" " if(!alreadyCreated) {\n" " std::map types;\n"; - BOOST_FOREACH(const Class& cls, classes) { + for(const Class& cls: classes) { if(cls.isVirtual) wrapperFile.oss << " types.insert(std::make_pair(typeid(" << cls.qualifiedName("::") << ").name(), \"" << cls.qualifiedName(".") << "\"));\n"; @@ -423,7 +421,7 @@ void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& modul " if(!registry)\n" " registry = mxCreateStructMatrix(1, 1, 0, NULL);\n" " typedef std::pair StringPair;\n" - " BOOST_FOREACH(const StringPair& rtti_matlab, types) {\n" + " for(const StringPair& rtti_matlab: types) {\n" " int fieldId = mxAddField(registry, rtti_matlab.first.c_str());\n" " if(fieldId < 0)\n" " mexErrMsgTxt(\"gtsam wrap: Error indexing RTTI types, inheritance will not work correctly\");\n" @@ -458,11 +456,11 @@ void Module::python_wrapper(const string& toolboxPath) const { wrapperFile.oss << "{\n"; // write out classes - BOOST_FOREACH(const Class& cls, expandedClasses) + for(const Class& cls: expandedClasses) cls.python_wrapper(wrapperFile); // write out global functions - BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions) + for(const GlobalFunctions::value_type& p: global_functions) p.second.python_wrapper(wrapperFile); // finish wrapper file diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h index 7718bc139..55e581ad8 100644 --- a/wrap/OverloadedFunction.h +++ b/wrap/OverloadedFunction.h @@ -49,7 +49,7 @@ public: std::vector expandArgumentListsTemplate( const TemplateSubstitution& ts) const { std::vector result; - BOOST_FOREACH(const ArgumentList& argList, argLists_) { + for(const ArgumentList& argList: argLists_) { ArgumentList instArgList = argList.expandTemplate(ts); result.push_back(instArgList); } @@ -63,8 +63,8 @@ public: void verifyArguments(const std::vector& validArgs, const std::string s) const { - BOOST_FOREACH(const ArgumentList& argList, argLists_) { - BOOST_FOREACH(Argument arg, argList) { + for(const ArgumentList& argList: argLists_) { + for(Argument arg: argList) { std::string fullType = arg.type.qualifiedName("::"); if (find(validArgs.begin(), validArgs.end(), fullType) == validArgs.end()) @@ -75,7 +75,7 @@ public: friend std::ostream& operator<<(std::ostream& os, const ArgumentOverloads& overloads) { - BOOST_FOREACH(const ArgumentList& argList, overloads.argLists_) + for(const ArgumentList& argList: overloads.argLists_) os << argList << std::endl; return os; } @@ -106,7 +106,7 @@ static std::map expandMethodTemplate( const std::map& methods, const TemplateSubstitution& ts) { std::map result; typedef std::pair NamedMethod; - BOOST_FOREACH(NamedMethod namedMethod, methods) { + for(NamedMethod namedMethod: methods) { F instMethod = namedMethod.second; instMethod.expandTemplate(ts); namedMethod.second = instMethod; @@ -119,7 +119,7 @@ template inline void verifyArguments(const std::vector& validArgs, const std::map& vt) { typedef typename std::map::value_type NamedMethod; - BOOST_FOREACH(const NamedMethod& namedMethod, vt) + for(const NamedMethod& namedMethod: vt) namedMethod.second.verifyArguments(validArgs); } diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index 41fd51680..1a1f1bc26 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -6,7 +6,6 @@ #include "ReturnType.h" #include "utilities.h" -#include #include using namespace std; diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 596acb2c3..1405e8e2b 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -8,7 +8,6 @@ #include "ReturnValue.h" #include "utilities.h" -#include #include using namespace std; diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index e4e7c89ae..23dc93d72 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -19,7 +19,6 @@ #include "StaticMethod.h" #include "utilities.h" -#include #include #include diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp index e925fd381..4c77d4e76 100644 --- a/wrap/TemplateInstantiationTypedef.cpp +++ b/wrap/TemplateInstantiationTypedef.cpp @@ -20,7 +20,6 @@ #include "utilities.h" #include -#include #include using namespace std; @@ -31,7 +30,7 @@ Class TemplateInstantiationTypedef::findAndExpand( const vector& classes) const { // Find matching class boost::optional matchedClass; - BOOST_FOREACH(const Class& cls, classes) { + for(const Class& cls: classes) { if (cls.name() == class_.name() && cls.namespaces() == class_.namespaces() && cls.templateArgs.size() == typeList.size()) { matchedClass.reset(cls); diff --git a/wrap/TypeAttributesTable.cpp b/wrap/TypeAttributesTable.cpp index 27de70d0e..f98e9b760 100644 --- a/wrap/TypeAttributesTable.cpp +++ b/wrap/TypeAttributesTable.cpp @@ -20,7 +20,6 @@ #include "Class.h" #include "utilities.h" -#include #include #include @@ -45,7 +44,7 @@ const TypeAttributes& TypeAttributesTable::attributes(const string& key) const { /* ************************************************************************* */ void TypeAttributesTable::addClasses(const vector& classes) { - BOOST_FOREACH(const Class& cls, classes) { + for(const Class& cls: classes) { if (!table_.insert( make_pair(cls.qualifiedName("::"), TypeAttributes(cls.isVirtual))).second) throw DuplicateDefinition("class " + cls.qualifiedName("::")); @@ -55,7 +54,7 @@ void TypeAttributesTable::addClasses(const vector& classes) { /* ************************************************************************* */ void TypeAttributesTable::addForwardDeclarations( const vector& forwardDecls) { - BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDecls) { + for(const ForwardDeclaration& fwDec: forwardDecls) { if (!table_.insert(make_pair(fwDec.name, TypeAttributes(fwDec.isVirtual))).second) throw DuplicateDefinition("class " + fwDec.name); } @@ -63,7 +62,7 @@ void TypeAttributesTable::addForwardDeclarations( /* ************************************************************************* */ void TypeAttributesTable::checkValidity(const vector& classes) const { - BOOST_FOREACH(const Class& cls, classes) { + for(const Class& cls: classes) { boost::optional parent = cls.qualifiedParent(); if (parent) { diff --git a/wrap/matlab.h b/wrap/matlab.h index 1639876cc..de3027eac 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -34,7 +34,6 @@ extern "C" { #include #include -#include #include #include @@ -61,15 +60,15 @@ using namespace boost; // not usual, but for conciseness of generated code // "Unique" key to signal calling the matlab object constructor with a raw pointer // to a shared pointer of the same C++ object type as the MATLAB type. // Also present in utilities.h -static const uint64_t ptr_constructor_key = - (uint64_t('G') << 56) | - (uint64_t('T') << 48) | - (uint64_t('S') << 40) | - (uint64_t('A') << 32) | - (uint64_t('M') << 24) | - (uint64_t('p') << 16) | - (uint64_t('t') << 8) | - (uint64_t('r')); +static const boost::uint64_t ptr_constructor_key = + (boost::uint64_t('G') << 56) | + (boost::uint64_t('T') << 48) | + (boost::uint64_t('S') << 40) | + (boost::uint64_t('A') << 32) | + (boost::uint64_t('M') << 24) | + (boost::uint64_t('p') << 16) | + (boost::uint64_t('t') << 8) | + (boost::uint64_t('r')); //***************************************************************************** // Utilities @@ -245,9 +244,9 @@ template T myGetScalar(const mxArray* array) { switch (mxGetClassID(array)) { case mxINT64_CLASS: - return (T) *(int64_t*) mxGetData(array); + return (T) *(boost::int64_t*) mxGetData(array); case mxUINT64_CLASS: - return (T) *(uint64_t*) mxGetData(array); + return (T) *(boost::uint64_t*) mxGetData(array); default: // hope for the best! return (T) mxGetScalar(array); @@ -350,7 +349,7 @@ mxArray* create_object(const std::string& classname, void *pointer, bool isVirtu int nargin = 2; // First input argument is pointer constructor key input[0] = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL); - *reinterpret_cast(mxGetData(input[0])) = ptr_constructor_key; + *reinterpret_cast(mxGetData(input[0])) = ptr_constructor_key; // Second input argument is the pointer input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast(mxGetData(input[1])) = pointer; diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 226030a0d..ef9051d14 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -1,6 +1,5 @@ #include #include -#include #include #include @@ -116,7 +115,7 @@ void _geometry_RTTIRegister() { if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; - BOOST_FOREACH(const StringPair& rtti_matlab, types) { + for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); if(fieldId < 0) mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp index 4b41f3958..8526900a7 100644 --- a/wrap/tests/expected2/geometry_wrapper.cpp +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -1,6 +1,5 @@ #include #include -#include #include @@ -91,7 +90,7 @@ void _geometry_RTTIRegister() { if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; - BOOST_FOREACH(const StringPair& rtti_matlab, types) { + for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); if(fieldId < 0) mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 6d22e1625..0f5c70348 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -1,6 +1,5 @@ #include #include -#include #include #include @@ -81,7 +80,7 @@ void _testNamespaces_RTTIRegister() { if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; - BOOST_FOREACH(const StringPair& rtti_matlab, types) { + for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); if(fieldId < 0) mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 2a525e08e..27c9be6d6 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -102,9 +102,19 @@ TEST( spirit, constMethod_p ) { EXPECT(parse("double norm() const;", constMethod_p, space_p).full); } + /* ************************************************************************* */ +/* See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56665 + GCC compiler issues with -O2 and -fno-strict-aliasing results in undefined + behaviour when spirit uses assign_a with a literal. + GCC versions 4.7.2 -> 5.4 inclusive */ + TEST( spirit, return_value_p ) { - bool isEigen = true; + static const bool T = true; + static const bool F = false; + + bool isEigen = T; + string actual_return_type; string actual_function_name; @@ -119,9 +129,9 @@ TEST( spirit, return_value_p ) { Rule funcName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; Rule returnType_p = - (basisType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]) | - (className_p[assign_a(actual_return_type)][assign_a(isEigen,false)]) | - (eigenType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]); + (basisType_p[assign_a(actual_return_type)][assign_a(isEigen, T)]) | + (className_p[assign_a(actual_return_type)][assign_a(isEigen, F)]) | + (eigenType_p[assign_a(actual_return_type)][assign_a(isEigen, T)]); Rule testFunc_p = returnType_p >> funcName_p[assign_a(actual_function_name)] >> str_p("();"); diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 57a5bce29..32ee85eee 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #include "utilities.h" @@ -71,9 +70,9 @@ bool assert_equal(const vector& expected, const vector& actual) } if(!match) { cout << "expected: " << endl; - BOOST_FOREACH(const vector::value_type& a, expected) { cout << "[" << a << "] "; } + for(const vector::value_type& a: expected) { cout << "[" << a << "] "; } cout << "\nactual: " << endl; - BOOST_FOREACH(const vector::value_type& a, actual) { cout << "[" << a << "] "; } + for(const vector::value_type& a: actual) { cout << "[" << a << "] "; } cout << endl; return false; } @@ -132,7 +131,7 @@ void createNamespaceStructure(const std::vector& namespaces, const std::string& toolboxPath) { using namespace boost::filesystem; path curPath = toolboxPath; - BOOST_FOREACH(const string& subdir, namespaces) { + for(const string& subdir: namespaces) { // curPath /= "+" + subdir; // original - resulted in valgrind error curPath = curPath / string(string("+") + subdir); if(!is_directory(curPath)) { diff --git a/wrap/utilities.h b/wrap/utilities.h index fbc8dc7a2..d8108d6a5 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -21,7 +21,6 @@ #include "FileWriter.h" #include -#include #include #include